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 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] class EasyType(Type):
    """
    Anonymous type which extends :class:`.Type` by automatically generating a unique name.

    .. code:: python

      CONF = EasyType()
    """
    _num = 0
    _template = '_ty%s'

    _const_template = '%s_%s'

    def __init__(self):
        name = self._template % EasyType._num
        EasyType._num += 1
        self.const_to_value = {}
        self.value_to_const = {}
        super(EasyType, self).__init__(name)

    def get_const(self, value):
        if not isinstance(value, collections.Hashable):
            raise ValueError('%s is not hashable' % value)
        if value not in self.value_to_const:

            name = self._const_template % (self.name, len(self.value_to_const))
            self.value_to_const[value] = Constant(name, self)
            self.const_to_value[self.value_to_const[value]] = value
        return self.value_to_const[value]

    def get_value(self, const):
        return self.const_to_value[const]

    def get_param(self):
        return EasyParameter(self)

    def __call__(self, *args):
        if len(args) == 0:
            return self.get_param()
        if len(args) == 1:
            return self.get_const(args[0])
        raise ValueError(args) class EasyParameter(Parameter):
    """
    Anonymous parameter which extends :class:`.Parameter` by automatically generating a unique name.

    .. code:: python

      CONF = EasyType()
      Q = EasyParameter(CONF)
    """
    _num = 0
    _template = 'x%s'

    def __init__(self, type):
        """
        :param type: the :class:`.Type` of the parameter
        """
        super(EasyParameter, self).__init__(
            self._template % EasyParameter._num, type)
        EasyParameter._num += 1 class EasyPredicate(Predicate):
    """
    Anonymous predicate P which extends :class:`.Predicate`.

    .. code:: python

      CONF = EasyType()
      AtConf = EasyPredicate(CONF)
    """
    num = 0
    template = '_pr%s'

    def __init__(self, *types):
        """
        :param types: a list of :class:`.Type` inputs to the predicate
        .. automethod:: __call__
        """
        name = self.template % EasyPredicate.num
        EasyPredicate.num += 1
        super(EasyPredicate, self).__init__(name, types) class Action(Operator):
    """
    PDDL action class.

    .. code:: python

      CONF = EasyType()
      AtConf = EasyPredicate(CONF, CONF)
      Q1, Q1 = EasyParameter(CONF), EasyParameter(CONF)

      a = Action(name='move', parameters=[Q1, Q2],
                 condition=AtConf(Q1),
                 effect=And(AtConf(Q2), Not(AtConf(Q1))))
    """

    def __init__(self, name, parameters, condition, effect, cost=None):
        """
        :param name: the name of the action
        :param parameters: a list of :class:`.Parameter`
        :param condition: a :class:`.Condition`
        :param effect: an :class:`.Effect`
        :param cost: a numeric cost
        """
        super(Action, self).__init__(parameters, condition, effect)
        self.name = name.lower()
        self.cost_included = False
        self.cost = cost

    def instantiate(self, args): return ActionInstance(self, args)

    def pddl(self, costs):
        s = '(:action ' + self.name + '\n'
        s += '\t:parameters (' + ' '.join(param.typed_pddl()
                                          for param in self.parameters) + ')\n'
        s += '\t:precondition ' + self.condition.pddl() + '\n'
        augmented_effect = self.effect
        if costs and self.cost is not None and not self.cost_included:
            augmented_effect = And(augmented_effect, Cost(self.cost))
        s += '\t:effect ' + augmented_effect.pddl()
        return s + ')'

    def to_strips(self, constants):
        condition = self.condition.dequantify(constants)
        effect = self.effect.dequantify(constants)
        all_parameters = condition.get_parameters() | effect.get_parameters()
        parameters = list(self.parameters) + \
            [p for p in all_parameters if p not in self.parameters]
        [effs] = effect.get_literals()
        for i, cons in enumerate(condition.get_literals()):
            instance = STRIPSAction('%s_%s' % (
                self.name, i), parameters, cons, effs)
            instance.original = self
            yield instance

    def clone(self):
        return Action(self.name[:], self.parameters[:], self.condition.clone(), self.effect.clone())

    def __repr__(self):
        return self.name
    __str__ = __repr__ class Axiom(Operator):
    """
    PDDL axiom (derived predicate).

    .. code:: python

      BLOCK, POSE = EasyType(), EasyType()
      AtPose = EasyPredicate(BLOCK, POSE)
      CollisionFree = EasyPredicate(POSE, POSE)
      Safe = EasyPredicate(BLOCK, POSE)
      B2 = EasyParameter(BLOCK)
      P1, P2 = EasyParameter(POSE), EasyParameter(POSE)

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

    def __init__(self, effect, condition):
        """
        :param effect: the :class:`.Atom` derived predicate
        :param condition: a :class:`.Condition`
        """
        if not isinstance(effect, Atom):
            raw_input('Axiom effect must be an Atom: %s' % effect)

        super(Axiom, self).__init__(effect.args, condition, effect)

    def instantiate(self, args): return AxiomInstance(self, args)

    def to_strips(self, constants):
        condition = self.condition.dequantify(constants)
        for i, cons in enumerate(condition.get_literals()):
            instance = STRIPSAxiom(cons, [self.effect])
            instance.original = self
            yield instance

    def pddl(self):
        return '(:derived %s\n\t%s)' % (self.effect.typed_pddl(), self.condition.pddl())

    def clone(self):
        return Axiom(self.condition.clone(), self.effect.clone())

    def __repr__(self):
        return self.effect.name
    __str__ = __repr__ class ForAll(Quantifier, Condition, Effect):
    """
    Universal quantifier (forall (X1 X2 ... XN) P).

    Example safe for all blocks formula:

    .. code:: python

      BLOCK, POSE = EasyType(), EasyType()
      Safe = EasyPredicate(BLOCK, POSE)
      B1, B2 = EasyParameter(BLOCK), EasyParameter(BLOCK)
      P1 = EasyParameter(POSE)

      f = ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))
    """
    _pddl_name = 'forall'
    _Connective = And

    def de_morgan(self, sign=True):
        formula = self.formula.de_morgan(sign=sign)
        return ForAll(self.args, formula) if sign else Exists(self.args, formula)

    def dequantify(self, constants):
        if has_existential_child(self.formula):
            raise NotImplementedError(
                'Currently do not support ForAll([...], Exists(...))')
        print 'Warning: ForAll propositionalized using a fixed set of constants'
        formulas = [f.dequantify(constants) for f in self.expand(constants)]
        return self._Connective(*formulas)

    def holds(self, atoms, constants):
        return all(formula.holds(atoms, constants) for formula in self.expand(constants))

    def positive_supporters(self, atoms, constants):
        literals = set()
        for formula in self.expand(constants):
            child_literals = formula.positive_supporters(atoms, constants)
            if child_literals is None:
                return None
            literals |= child_literals
        return literals

    def negative_supporters(self, atoms, constants):
        for formula in self.expand(constants):
            child_literals = formula.negative_supporters(atoms, constants)
            if child_literals is not None:
                return child_literals
        return None

    def add(self, atoms, constants):
        return [formula.add(atoms, constants) for formula in self.expand(constants)]

    def delete(self, atoms, constants):
        return [formula.delete(atoms, constants) for formula in self.expand(constants)] class Exists(Quantifier, Condition):
    """
    Existential quantifier (exists (X1 X2 ... XN) P).

    Example collision free for some pose formula:

    .. code:: python

      BLOCK, POSE = EasyType(), EasyType()
      AtPose = EasyPredicate(BLOCK, POSE)
      CollisionFree = EasyPredicate(POSE, POSE)
      B2 = EasyParameter(BLOCK)
      P1, P2 = EasyParameter(POSE), EasyParameter(POSE)

      f = Exists([P2], And(AtPose(B2, P2), CollisionFree(P1, P2)))
    """
    _pddl_name = 'exists'
    _Connective = Or

    def de_morgan(self, sign=True):
        formula = self.formula.de_morgan(sign=sign)
        return Exists(self.args, formula) if sign else ForAll(self.args, formula)

    def dequantify(self, constants):
        return self.formula.dequantify(constants)

    def holds(self, atoms, constants):
        return any(formula.holds(atoms, constants) for formula in self.expand(constants))

    def positive_supporters(self, atoms, constants):
        for formula in self.expand(constants):
            child_literals = formula.positive_supporters(atoms, constants)
            if child_literals is not None:
                return child_literals
        return None

    def negative_supporters(self, atoms, constants):
        literals = set()
        for formula in self.expand(constants):
            child_literals = formula.negative_supporters(atoms, constants)
            if child_literals is None:
                return None
            literals |= child_literals
        return literals class Not(Connective, Condition, Effect):
    """
    Logical negation (not P).

    .. code:: python

      CONF = EasyType()
      AtConf = EasyPredicate(CONF, CONF)
      Q1 = EasyParameter(CONF)

      f = Not(AtConf(Q1)))
    """
    _pddl_name = 'not'

    def __init__(self, formula):
        """
        :param formula: a :class:`.Formula`
        """
        super(Not, self).__init__(formula)

    @property
    def formula(self): return self.formulas[0]

    def de_morgan(self, sign=True): return self.formula.de_morgan(
        sign=not sign)

    def simplify(self): return self

    def get_literals(self):
        dnf = self.formulas[0].get_literals()
        for combo in product(*dnf):
            yield map(utils.invert, combo)

    def holds(self, atoms, constants):
        return not self.formula.holds(atoms, constants)

    def positive_supporters(self, atoms, constants):
        return set(map(utils.invert, self.formula.negative_supporters(atoms, constants)))

    def negative_supporters(self, atoms, constants):
        return set(map(utils.invert, self.formula.positive_supporters(atoms, constants)))

    def add(self, atoms, constants):
        return self.formula.delete(atoms, constants)

    def delete(self, atoms, constants):
        return self.formula.add(atoms, constants)

    def __eq__(self, other):
        return type(self) == type(other) and self.formula == other.formula

    def __ne__(self, other):
        return not(self == other)

    def __hash__(self):
        return hash((self.__class__, self.formulas)) class And(Connective, Condition, Effect):
    """
    Logical conjunction (and P1 P2 ... PN).

    .. code:: python

      BLOCK, POSE = EasyType(), EasyType()
      AtPose = EasyPredicate(BLOCK, POSE)
      CollisionFree = EasyPredicate(POSE, POSE)
      B2 = EasyParameter(BLOCK)
      P1, P2 = EasyParameter(POSE), EasyParameter(POSE)

      f = And(AtPose(B2, P2), CollisionFree(P1, P2))
    """
    _pddl_name = 'and'

    def de_morgan(self, sign=True):
        formulas = [f.de_morgan(sign=sign) for f in self.formulas]

        return And(*formulas) if sign else Or(*formulas)

    def get_literals(self):
        dnfs = [f.get_literals() for f in self.formulas]
        for combo in product(*dnfs):

            yield reduce(operator.add, combo)

    def holds(self, atoms, constants):
        return all(formula.holds(atoms, constants) for formula in self.formulas)

    def positive_supporters(self, atoms, constants):
        literals = set()
        for formula in self.formulas:
            child_literals = formula.positive_supporters(atoms, constants)
            if child_literals is None:
                return None
            literals |= child_literals
        return literals

    def negative_supporters(self, atoms, constants):
        for formula in self.formulas:
            child_literals = formula.negative_supporters(atoms, constants)
            if child_literals is not None:
                return child_literals
        return None

    def add(self, atoms, constants):
        return flatten(formula.add(atoms, constants) for formula in self.formulas)

    def delete(self, atoms, constants):
        return flatten(formula.delete(atoms, constants) for formula in self.formulas) class Or(Connective, Condition):
    """
    Logical disjunction (or P1 P2 ... PN).

    .. code:: python

      BLOCK, POSE = EasyType(), EasyType()
      CollisionFree = EasyPredicate(BLOCK, POSE)
      B1, B2 = EasyParameter(BLOCK), EasyParameter(BLOCK)
      P1, P2 = EasyParameter(POSE)

      f = Or(Equal(B1, B2), Safe(B2, P1)))
    """
    _pddl_name = 'or'

    def de_morgan(self, sign=True):
        formulas = [f.de_morgan(sign=sign) for f in self.formulas]

        return Or(*formulas) if sign else And(*formulas)

    def get_literals(self):
        return flatten(f.get_literals() for f in self.formulas)

    def holds(self, atoms, constants):
        return any(formula.holds(atoms, constants) for formula in self.formulas)

    def positive_supporters(self, atoms, constants):
        for formula in self.formulas:
            child_literals = formula.positive_supporters(atoms, constants)
            if child_literals is not None:
                return child_literals
        return None

    def negative_supporters(self, atoms, constants):
        literals = set()
        for formula in self.formulas:
            child_literals = formula.negative_supporters(atoms, constants)
            if child_literals is None:
                return None
            literals |= child_literals
        return literals class Equal(Formula, Condition):
    """
    Equality atom (= O1 O2 ... ON).

    .. code:: python

      BLOCK = EasyType()
      B1, B2 = EasyParameter(BLOCK), EasyParameter(BLOCK)

      f = Equal(B1, B2)
    """

    def __init__(self, arg1, arg2, *args):
        """
        :param arg1: the first :class:`.Object` argument
        :param arg2: the second :class:`.Object` argument
        :param *args: a list of any remaining :class:`.Object` arguments
        """
        self.args = [arg1, arg2] + list(args)
        if not all(isinstance(arg, Object) for arg in self.args):
            raise ValueError(
                'Expected atom arguments %s to extend class Object' % self.args)

    def get_atoms(self): return set()

    def get_literals(self):
        constants = filter(lambda a: isinstance(a, Constant), self.args)
        if constants and not all(c == constants[0] for c in constants):
            return []
        params = filter(lambda a: not isinstance(a, Constant), self.args)
        if not params:
            return [[]]
        if constants:
            return [[Equal(constants[0], *params)]]
        return [[Equal(*params)]]

    def get_formulas(self): return []

    def get_quantified(self): return set()

    def substitute(self, atom, subformula): pass

    def propositional(self, constants): return self

    def dequantify(self, constants): return self

    def instantiate(self, parameter_map):
        return self.__class__(*[parameter_map.get(arg, arg) for arg in self.args])

    def de_morgan(self, sign=True):
        return self if sign else stripstream.pddl.logic.connectives.Not(self)

    def holds(self, atoms, constants):

        return all(self.args[0] == arg for arg in self.args)

    def positive_supporters(self, atoms, constants):
        return set() if self.holds(atoms, constants) else None

    def negative_supporters(self, atoms, constants):
        raise None if self.holds(atoms, constants) else set()

    def pddl(self):
        return '(= %s)' % ' '.join(arg.pddl() for arg in self.args)
    __repr__ = pddl def rename_easy(assignments):
    """
    Mutates any :class:`.EasyType`, :class:`.EasyPredicate`, or :class:`.EasyParameter` instance to assign their local scope name

    :param assignments: the result of ```locals()```
    """
    for name, value in assignments.iteritems():
        if type(value) in [EasyType, EasyPredicate]:
            value.name = name.lower()
        elif type(value) == EasyParameter:
            Parameter.__init__(value, name, value.type) class STRIPStreamProblem(object):

    def __init__(self, initial_atoms, goal_literals, operators, cond_streams, objects=[]):

        self.initial_atoms = list(initial_atoms)
        if isinstance(goal_literals, Iterable):
            self.goal_literals = And(*goal_literals)
        elif isinstance(goal_literals, Formula):
            self.goal_literals = goal_literals

        else:
            raise ValueError(goal_literals)
        self.operators = operators
        self.cond_streams = cond_streams
        self.objects = objects

    def is_strips(self):
        return all(isinstance(op, STRIPSAction) or
                   isinstance(op, STRIPSAxiom) for op in self.operators)

    def convert_goal(self, axiom=True):
        from stripstream.pddl.logic.predicates import NamedPredicate
        from stripstream.pddl.operators import Action, Axiom
        Goal = NamedPredicate('_goal')
        if axiom:
            operator = Axiom(effect=Goal(), condition=self.goal_literals)
        else:
            operator = Action(name='_achieve_goal', parameters=[],
                              condition=self.goal_literals, effect=Goal(), cost=None)
        operator.is_internal = True
        self.operators.append(operator)
        self.goal_literals = Goal()

    def replace_axioms(self):
        derived = {}
        for op in self.operators:
            if isinstance(op, Axiom):
                derived[op.effect.predicate] = op
        actions = []
        for action in self.operators:
            if isinstance(action, Action):
                substitute_axioms(action.condition, derived)
                actions.append(action)
        self.operators = actions
        substitute_axioms(self.goal_literals, derived)

    def convert_axioms_to_effects(self):
        derived_predicates = self.get_derived_predicates()
        fluent_predicates = self.get_fluent_predicates()
        constants = defaultdict(set)
        for const in self.get_constants():
            constants[const.type].add(const)

        mapping = []
        for op in self.operators:
            if isinstance(op, Axiom):
                [conditions] = op.condition.dequantify(
                    constants).get_literals()
                assert not filter(
                    lambda a: a.predicate in derived_predicates, conditions)
                print conditions
                [fluent] = filter(
                    lambda a: a.predicate in fluent_predicates, conditions)
                others = filter(lambda a: a != fluent, conditions)
                mapping.append((fluent, others, op.effect))

        new_operators = []
        for op in self.operators:
            if isinstance(op, Action):
                new_op = op.clone()
                new_operators.append(new_op)
                [literals] = new_op.effect.get_literals()
                for literal in literals:
                    effect = literal.formula if isinstance(
                        literal, Not) else literal
                    for fluent, conditions, derived in mapping:
                        if effect.predicate == fluent.predicate:
                            free_params = set(
                                flatten(map(lambda a: a.args, [derived] + conditions))) - set(fluent.args)
                            param_map = dict(zip(fluent.args, effect.args))
                            for i, param in enumerate(free_params):

                                param_map[param] = Parameter(
                                    'l%s' % i, param.type)

                            new_params = list(
                                set(param_map.values()) - set(effect.args))
                            new_derived = derived.instantiate(param_map)
                            new_conditions = [cond.instantiate(
                                param_map) for cond in conditions]
                            if isinstance(literal, Not):
                                new_op.add_effects(ForAll(new_params, When(
                                    And(*new_conditions), new_derived)))
                            else:
                                new_op.add_effects(ForAll(new_params, When(
                                    And(*new_conditions), Not(new_derived))))

        return new_operators

    def to_strips(self):
        constants = defaultdict(set)
        for const in self.get_constants():
            constants[const.type].add(const)
        new_operators = []
        for operator in self.operators:
            for strips in operator.to_strips(constants):
                new_operators.append(strips)
        self.operators = new_operators

    def new_problem(self, initial_atoms):
        return STRIPStreamProblem(initial_atoms, self.goal_literals, self.operators, self.cond_streams, self.objects)

    def get_constants(self):
        objects = set(self.objects)
        for atom in self.initial_atoms:
            objects.update(atom.args)
        if isinstance(self.goal_literals, Formula):
            for atom in self.goal_literals.get_atoms():
                objects.update(atom.args)

        for operator in self.operators:
            for atom in operator.condition.get_atoms() | operator.effect.get_atoms():
                objects.update(atom.args)
        for cs in self.cond_streams:
            for atom in cs.conditions + cs.effects:
                objects.update(atom.args)
        return filter(lambda o: isinstance(o, Constant), objects)

    def get_fluent_predicates(self):
        from stripstream.pddl.operators import Action
        fluents = set()
        for operator in self.operators:
            if isinstance(operator, Action):
                fluents.update(
                    {atom.predicate for atom in operator.effect.get_atoms()})
        return fluents

    def get_derived_predicates(self):
        from stripstream.pddl.operators import Axiom
        return set(operator.effect.predicate for operator in self.operators if isinstance(operator, Axiom))

    def get_stream_predicates(self):
        return {atom.predicate for cs in self.cond_streams for atom in cs.conditions + cs.effects}

    def get_predicates(self):
        return {atom.predicate for op in self.operators for atom in op.condition.get_atoms() | op.effect.get_atoms()} | {atom.predicate for cs in self.cond_streams for atom in cs.conditions + cs.effects} | {atom.predicate for atom in self.initial_atoms} | {atom.predicate for atom in self.goal_literals.get_atoms()}

    def get_static_predicates(self):
        return self.get_predicates() - self.get_fluent_predicates() - self.get_derived_predicates() - self.get_stream_predicates()

    def __repr__(self):
        return 'STRIPStream Problem\n'           'Initial: %s\n'           'Goal: %s\n'           'Operators: %s\n'           'Streams: %s\n'           'Constants: %s' % (self.initial_atoms, self.goal_literals,
                                                                                                                                                                           self.operators, self.cond_streams, self.objects) class EasyTestStream(CondStream):
    """
    Conditional stream given by a test.

    Example for collision checking:

    .. code:: python

      POSE = EasyType()
      CollisionFree = EasyPredicate(POSE, POSE)
      P1, P1 = EasyParameter(POSE), EasyParameter(POSE)

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

    def __init__(self, inputs, conditions, effects, test, **kwargs):
        """
        :param inputs: a list of :class:`.Parameter` which are the inputs to ``test``
        :param conditions: a list of :class:`.Atom` forming a conjunctive condition on inputs
        :param effects: a list of :class:`.Atom` forming a conjunctive effect on inputs
        :param test: a function from values for ``inputs`` to ``{False, True}``
        :param kwargs: keyword arguments for :class:`.ConditionalStream`
        """
        super(EasyTestStream, self).__init__(inputs, [],
                                             conditions, effects, cost=TEST_COST, **kwargs)
        if not callable(test):
            raise ValueError(
                'EasyTestStream expects test to be a function: %s' % test)
        self.test = test

    class StreamFn(TestStream):

        def test(self, inputs):
            values = tuple(inp.type.get_value(inp) for inp in inputs)
            truth = self.cond_stream.test(*values)
            if truth not in (True, False):
                raise ValueError(
                    'Expected boolean test output but received %s' % truth)
            return truth class EasyGenStream(CondStream):
    """
    Conditional stream given by a generator.

    Example for inverse kinematics:

    .. code:: python

      CONF, POSE = EasyType(), EasyType()
      LegalKin = EasyPredicate(POSE, CONF)
      Q, P = EasyParameter(CONF), EasyParameter(POSE)

      cs = EasyGenStream(inputs=[P], outputs=[Q], conditions=[], effects=[LegalKin(P, Q)],
                         generator=lambda p: iter([p]))
    """

    def __init__(self, inputs, outputs, conditions, effects, generator, **kwargs):
        """
        :param inputs: a list of :class:`.Parameter` which are the inputs to ``generator``
        :param outputs: a list of :class:`.Parameter` which are the outputs from ``generator``
        :param conditions: a list of :class:`.Atom` forming a conjunctive condition on inputs
        :param effects: a list of :class:`.Atom` forming a conjunctive effect on inputs and outputs
        :param generator: a function from values for ``inputs`` to a generator for values of ``outputs``
        :param kwargs: keyword arguments for :class:`.ConditionalStream`
        """
        super(EasyGenStream, self).__init__(
            inputs, outputs, conditions, effects, **kwargs)

        if not callable(generator):
            raise ValueError(
                'EasyGenStream expects generator to be a function: %s' % generator)
        self.generator = generator

    class StreamFn(GeneratorStream):

        def get_generator(self, inputs):
            sequence = self.cond_stream.generator(
                *tuple(inp.type.get_value(inp) for inp in inputs))

            for i, outputs in enumerate(sequence):

                if len(outputs) != len(self.cond_stream.outputs):
                    raise ValueError('Expected generator output %s to be the same length as conditional stream inputs %s' % (
                        outputs, self.cond_stream.outputs))
                yield [tuple(par.type(out) for par, out in zip(self.cond_stream.outputs, outputs))] SEPARATOR = '\n' + 85 * '-' + '\n' def dantam2():
    m, n = 3, 3

    n_obj = 2
    side_dim = .07
    height_dim = .1
    box_dims = (side_dim, side_dim, height_dim)
    separation = (side_dim, side_dim)

    workspace = np.array([[-2.0, -1.5, 0.0],
                          [1.0, 1.5, 2.0]])
    robot = makeRobot(workspace, useLeft=True, useRight=False)
    base_conf = (-.75, .2, -math.pi / 2)
    initial_conf = robot.makeConf(
        *base_conf, g=robot.gripMax, ignoreStandard=True)
    initial_conf = initial_conf.set('pr2LeftArm', TOP_HOLDING_LEFT_ARM)

    initial_poses = []
    goal_poses = {}

    length = m * (box_dims[0] + separation[0])
    width = n * (box_dims[1] + separation[1])
    height = .7
    table = get_box_body(length, width, height, name='table', color='brown')
    initial_poses.append((table, hu.Pose(0, 0, height / 2, 0)))

    poses = []
    z = height + height_dim / 2 + BODY_PLACEMENT_Z_OFFSET
    theta = 0
    for r in range(m):
        row = []
        x = -length / 2 + (r + .5) * (box_dims[0] + separation[0])
        for c in range(n):
            y = -width / 2 + (c + .5) * (box_dims[1] + separation[1])
            row.append(hu.Pose(x, y, z, theta))
        poses.append(row)

    coordinates = list(product(range(m), range(n)))
    assert n_obj <= len(coordinates)
    obj_coordinates = sample(coordinates, n_obj)
    movable_names = []
    for i, (r, c) in enumerate(obj_coordinates):
        row_color = np.zeros(4)
        row_color[2 - r] = 1.
        if i == 0:
            name = 'goal%d-%d' % (r, c)
            color = 'blue'

            goal_poses[name] = poses[m / 2][n / 2]
        else:
            name = 'block%d-%d' % (r, c)
            color = 'red'

        obj = get_box_body(*box_dims, name=name, color=color)
        initial_poses.append((obj, poses[r][c]))
        movable_names.append(name)

    known_poses = [pose for col in poses for pose in col]

    return ManipulationProblem(workspace, initial_conf, initial_poses=initial_poses,
                               movable_names=movable_names, known_poses=known_poses,
                               goal_poses=goal_poses, goal_conf=initial_conf) def is_window_active(window_3d):
    return window_3d.window is not None def inverse_kinematics(default_conf, manip_trans, arm):
    return default_conf.robot.inverseKinWristGen(manip_trans, arm, default_conf,
                                                 basePose=default_conf.basePose()) class Traj(object):
    _ids = count(0)

    def __init__(self, path, grasp=None):
        self.id = next(self._ids)
        self.path = path
        self.grasp = grasp

    def __repr__(self):
        return '%s(%d)' % (self.__class__.__name__, self.id) def manip_from_pose_grasp(pose, grasp, robot, hand):
    return pose.compose(get_wrist_frame(grasp, robot, hand)) def approach_from_manip(manip_trans):
    if abs(manip_trans.matrix[2, 0]) < 0.1:
        return manip_trans.compose(hu.Pose(-BACKOFF, 0., PERP_BACKOFF, 0.))
    return manip_trans.compose(hu.Pose(-BACKOFF, 0., 0., 0.)) def get_name(shape):
    return shape.name() def get_wrist_frame(grasp, robot, hand):
    gripperFrame = robot.gripperFaceFrame[hand]
    wristFrame = grasp.compose(gripperFrame.inverse())
    return wristFrame def get_sample_fn(conf, active_chains):
    return lambda: conf.robot.randomConf(active_chains, conf) def get_extend_fn(active_chains, step_size):

    return lambda q1, q2: interpolate(q1, q2, step_size, active_chains) def get_distance_fn(active_chains):
    def distance_fn(q1, q2):
        assert q1.robot == q2.robot
        return q1.robot.distConf(q1, q2, active_chains)

    return distance_fn def get_collision_fn(placed=[], attached=None):

    def collision_fn(q):
        placed_robot = q.placement(attached)

        return any(placed_robot.collides(body) for body in placed)
    return collision_fn def draw_window_3d(window_3d, robot_conf, placements, attached=None):
    if not is_window_active(window_3d):
        return
    window_3d.clear()
    placed_robot = robot_conf.placement(attached=attached)
    window_3d.draw(placed_robot, color=ROBOT_COLOR)
    for placed in placements:
        window_3d.draw(placed, color=get_color(placed)) TOP_GRASPS = map(get_grasp_frame_origin, [gMat2, gMat3])
from time import sleep
from PyR2.graphics3D import Window3D
from stripstream.algorithms.incremental.incremental_planner import incremental_planner
from stripstream.algorithms.search.fast_downward import get_fast_downward
from stripstream.pddl.utils import convert_plan
from stripstream.pddl.objects import EasyType, EasyParameter
from stripstream.pddl.logic.predicates import EasyPredicate
from stripstream.pddl.operators import Action, Axiom
from stripstream.pddl.logic.quantifiers import ForAll, Exists
from stripstream.pddl.logic.connectives import Not, And, Or
from stripstream.pddl.logic.atoms import Equal
from stripstream.pddl.utils import rename_easy
from stripstream.pddl.problem import STRIPStreamProblem
from stripstream.pddl.cond_streams import EasyTestStream, EasyGenStream
from stripstream.utils import SEPARATOR
from robotics.pyr2.problems import dantam2
from robotics.pyr2.utils import is_window_active, inverse_kinematics, Traj, \
    manip_from_pose_grasp, approach_from_manip, get_name, get_wrist_frame, get_sample_fn, get_extend_fn, \
    get_distance_fn, get_collision_fn, draw_window_3d, TOP_GRASPS
from motion_planners.rrt_connect import birrt, direct_path
        [P2], And(PoseEq(O2, P2), CFreePose(P, P2)))),
    Axiom(effect=SafeTraj(O2, T), condition=Exists(
        [P2], And(PoseEq(O2, P2), CFreeTraj(T, P2)))),
]

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

# TODO - self.robot.selfCollidePairs (in objects.pyx)


def amber_stripstream(use_window=True, step=False):
    problem = dantam2()
    window_3d = Window3D(title='World', windowWidth=500, noWindow=(not use_window),
                         viewport=problem.workspace.flatten('F'))

    if is_window_active(window_3d):
        draw_window_3d(window_3d, problem.initial_conf,
                       [body.applyTrans(pose) for body, pose in problem.initial_poses])
        raw_input('Start?')

    robot = problem.initial_conf.robot
    name_to_body = {get_name(body): body for body, _ in problem.initial_poses}
    static_bodies = [body.applyTrans(pose) for body, pose in problem.initial_poses
                     if get_name(body) not in problem.movable_names]
    block = name_to_body[problem.movable_names[-1]]  # Generic object

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

    # NOTE - can either modify the world state or create new bodies
    # Collision free test between an object at pose1 and an object at pose2
    def cfree_pose(pose1, pose2):
        return not ((pose1 == pose2) or
                    block.applyTrans(pose1).collides(block.applyTrans(pose2)))

    def cfree_traj(traj, pose):  # Collision free test between a robot executing traj (which may or may not involve a grasp) and an object at pose
        if DISABLE_TRAJ_COLLISIONS:
            return True
        placed = [body.applyTrans(pose)]
        attached = None
        if traj.grasp is not None:
            attached = {ARM: body.applyTrans(
                get_wrist_frame(traj.grasp, robot, ARM).inverse())}
        # TODO - cache bodies for these configurations
        collision_fn = get_collision_fn(placed, attached)
        return not any(collision_fn(q) for q in traj.path)

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

    # Sample pregrasp config and motion plan that performs a grasp
    def sample_grasp_traj(pose, grasp):
        attached = {ARM: body.applyTrans(
            get_wrist_frame(grasp, robot, ARM).inverse())}
        collision_fn = get_collision_fn(static_bodies, attached)

        manip_frame = manip_from_pose_grasp(pose, grasp, robot, ARM)
        grasp_conf = next(inverse_kinematics(
            problem.initial_conf, manip_frame, ARM), None)  # Grasp configuration
        if grasp_conf is None or collision_fn(grasp_conf):
            return

        # NOTE - I could also do this in the opposite order
        pregrasp_frame = approach_from_manip(manip_frame)
        pregrasp_conf = next(inverse_kinematics(
            grasp_conf, pregrasp_frame, ARM))  # Pregrasp configuration
        if pregrasp_conf is None or collision_fn(pregrasp_conf):
            return

        if DISABLE_GRASP_TRAJECTORIES:
            yield (pregrasp_conf, Traj([grasp_conf], grasp))
            return
        grasp_path = direct_path(grasp_conf, pregrasp_conf,  # Trajectory from grasp configuration to pregrasp
                                 get_extend_fn([robot.armChainNames[ARM]], STEP_SIZE), collision_fn)
        if grasp_path is None:
            return
        yield (pregrasp_conf, Traj(grasp_path, grasp))

    def sample_free_motion(conf1, conf2):  # Sample motion while not holding
        if DISABLE_MOTION_TRAJECTORIES:
            yield (Traj([conf2]),)
            return
        active_chains = [robot.armChainNames[ARM]]
        path = birrt(conf1, conf2, get_distance_fn(active_chains),
                     get_sample_fn(conf1, active_chains),
                     get_extend_fn(active_chains, STEP_SIZE), get_collision_fn(
                         static_bodies),
                     restarts=0, iterations=50, smooth=None)
        if path is None:
            return
        yield (Traj(path),)

    def sample_holding_motion(conf1, conf2, grasp):  # Sample motion while holding
        if DISABLE_MOTION_TRAJECTORIES:
            yield (Traj([conf2], grasp),)
            return
        active_chains = [robot.armChainNames[ARM]]
        attached = {ARM: body.applyTrans(
            get_wrist_frame(grasp, robot, ARM).inverse())}
        path = birrt(conf1, conf2, get_distance_fn(active_chains),
                     get_sample_fn(conf1, active_chains),
                     get_extend_fn(active_chains, STEP_SIZE), get_collision_fn(
                         static_bodies, attached),
                     restarts=0, iterations=50, smooth=None)
        if path is None:
            return
        yield (Traj(path, grasp),)

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

    cond_streams = [
        # Pick/place trajectory
        EasyGenStream(inputs=[P, G], outputs=[Q, T], conditions=[],
                      effects=[GraspMotion(P, G, Q, T)], generator=sample_grasp_traj),

        # Move trajectory
        EasyGenStream(inputs=[Q, Q2], outputs=[T], conditions=[],
                      effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0),
        EasyGenStream(inputs=[Q, Q2, G], outputs=[T], conditions=[],
                      effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion, order=1, max_level=0),

        # Collisions

        EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePose(P, P2)],