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 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 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 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 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 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 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 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 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 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) 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) 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 convert_plan(plan):
    if plan is None:
        return plan
    return [(action, map(get_value, args)) for action, args in plan]
from stripstream.pddl.logic.predicates import EasyPredicate as Pred
from stripstream.pddl.objects import EasyType as Type, EasyParameter as Param
from stripstream.pddl.logic.connectives import Not, Or, And
from stripstream.pddl.logic.quantifiers import Exists, ForAll
from stripstream.pddl.logic.atoms import Equal
from stripstream.pddl.operators import Action, Axiom
from stripstream.pddl.problem import STRIPStreamProblem
from stripstream.pddl.utils import rename_easy
from itertools import product
from stripstream.algorithms.incremental.incremental_planner import incremental_planner
from stripstream.pddl.utils import convert_plan
#!/usr/bin/env python



def create_problem():
    """
    Creates the 1D task and motion planning STRIPStream problem.
    This models the same problem as :module:`.run_tutorial` but does so without using any streams.

    :return: a :class:`.STRIPStreamProblem`
    """

    num_blocks = 3
    blocks = ['block%i' % i for i in range(num_blocks)]
    num_poses = num_blocks + 1

    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)}

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

    # Data types
    CONF, BLOCK, POSE = Type(), Type(), Type()

    # Fluent predicates
    AtConf = Pred(CONF)
    AtPose = Pred(BLOCK, POSE)
    HandEmpty = Pred()
    Holding = Pred(BLOCK)

    # Derived predicates
    Safe = Pred(BLOCK, BLOCK, POSE)

    # Static predicates
    LegalKin = Pred(POSE, CONF)
    CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)

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

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

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

    actions = [

        Action(name='pick', parameters=[B1, P1, Q1],