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__ def irange(start, end, step=1):
    n = start
    while n < end:
        yield n
        n += step INF = float('inf') 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 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))] 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 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) def are_colliding(block1, x1, block2, x2):
    sleep(COLLISION_TIME)
    if x1 == x2:
        return True
    return not (x1 + block1.w / 2. < x2 - block2.w / 2. or
                x1 - block1.w / 2. > x2 + block2.w / 2.) def in_region(region, block, x):
    sleep(CONTAIN_TIME)
    return region.x - region.w / 2. <= x - block.w / 2 and region.x + region.w / 2. >= x + block.w / 2 def sample_region_pose(region, block):
    sleep(REGION_TIME)
    return region.x + (region.w - block.w) * (random() - .5) def inverse_kinematics(pose):
    sleep(IK_TIME)
    return pose def get_value(x):
    if type(x) != Constant or not isinstance(x.type, EasyType):
        return x
    return x.type.get_value(x)
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.utils import irange, INF
from stripstream.pddl.utils import rename_easy
from stripstream.pddl.problem import STRIPStreamProblem
from stripstream.pddl.cond_streams import EasyGenStream, EasyTestStream
from stripstream.pddl.objects import EasyType as Type, EasyParameter as Param
from stripstream.pddl.logic.predicates import EasyPredicate as Pred
from stripstream.pddl.examples.continuous_tamp.continuous_tamp_utils import are_colliding, in_region,  sample_region_pose, inverse_kinematics
from stripstream.pddl.utils import get_value
    from stripstream.pddl.examples.continuous_tamp.continuous_tamp_viewer import ContinuousTMPViewer
    from stripstream.pddl.examples.continuous_tamp.continuous_tamp_viewer import ContinuousTMPViewer

EAGER_TESTS = True

CONF, BLOCK, POSE, REGION = Type(), Type(), Type(), Type()

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

Safe = Pred(BLOCK, BLOCK, POSE)
InRegion = Pred(BLOCK, REGION)


LegalKin = Pred(POSE, CONF)
CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)
Contained = Pred(BLOCK, POSE, REGION)
CanPlace = Pred(BLOCK, REGION)


IsSink = Pred(REGION)