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

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

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

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

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

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

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

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

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

        bound_plan = produce_bindings(
            universe, order, plan, greedy, shared, dfs)
        if verbose:
            print 'Bound plan:', bound_plan
        if bound_plan is not None:
            assert is_real_solution(universe, bound_plan)
            return bound_plan, universe
    return None, universe def get_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] def dantam_distract(env, n_obj):
    env.Load(os.path.join(ENVIRONMENTS_DIR, 'empty.xml'))

    m, n = 3, 3
    side_dim = .07
    height_dim = .1
    box_dims = (side_dim, side_dim, height_dim)
    separation = (side_dim, side_dim)

    coordinates = list(product(range(m), range(n)))
    assert n_obj <= len(coordinates)
    obj_coordinates = sample(coordinates, n_obj)

    length = m * (box_dims[0] + separation[0])
    width = n * (box_dims[1] + separation[1])
    height = .7
    table = box_body(env, 'table', length, width, height, color=TAN)
    set_pose(table, pose_from_quat_point(unit_quat(), np.array([0, 0, 0])))
    env.Add(table)

    robot = env.GetRobots()[0]
    set_manipulator_conf(robot.GetManipulator('leftarm'), TOP_HOLDING_LEFT_ARM)
    open_gripper(robot.GetManipulator('leftarm'))
    set_manipulator_conf(robot.GetManipulator('rightarm'),
                         mirror_arm_config(robot, REST_LEFT_ARM))
    close_gripper(robot.GetManipulator('rightarm'))
    robot.SetDOFValues([.15], [robot.GetJointIndex('torso_lift_joint')])
    set_base_conf(robot, (-.75, .2, -math.pi / 2))

    poses = []
    z = height + SURFACE_Z_OFFSET
    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(Pose(pose_from_quat_point(
                unit_quat(), np.array([x, y, z]))))
        poses.append(row)

    initial_poses = {}
    goal_poses = {}
    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
        initial_poses[name] = poses[r][c]
        obj = box_body(env, name, *box_dims, color=color)
        set_pose(obj, poses[r][c].value)
        env.Add(obj)

    known_poses = list(flatten(poses))
    object_names = initial_poses.keys()
    known_grasps = list(
        map(Grasp, top_grasps(env.GetKinBody(object_names[0]))))

    return ManipulationProblem(object_names=object_names, table_names=[table.GetName()],
                               goal_poses=goal_poses, initial_poses=initial_poses, known_poses=known_poses, known_grasps=known_grasps) def open_gripper(manipulator):
    _, upper = manipulator.GetRobot().GetDOFLimits(manipulator.GetGripperIndices())
    set_gripper(manipulator, upper) class Conf(Wrapper):
    _ids = count(0) def initialize_openrave(env, manipulator_name, min_delta=MIN_DELTA, collision_checker='ode'):
    env.StopSimulation()
    for sensor in env.GetSensors():
        sensor.Configure(Sensor.ConfigureCommand.PowerOff)
        sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
        sensor.Configure(Sensor.ConfigureCommand.RenderGeometryOff)
        env.Remove(sensor)
    env.SetCollisionChecker(RaveCreateCollisionChecker(env, collision_checker))
    env.GetCollisionChecker().SetCollisionOptions(0)

    assert len(env.GetRobots()) == 1
    robot = env.GetRobots()[0]

    cd_model = databases.convexdecomposition.ConvexDecompositionModel(robot)
    if not cd_model.load():
        print 'Generating convex decomposition model'
        cd_model.autogenerate()
    l_model = databases.linkstatistics.LinkStatisticsModel(robot)

    if not l_model.load():
        print 'Generating link statistics model'
        l_model.autogenerate()
    l_model.setRobotWeights()
    l_model.setRobotResolutions(xyzdelta=min_delta)

    robot.SetActiveManipulator(manipulator_name)
    manipulator = robot.GetManipulator(manipulator_name)
    robot.SetActiveDOFs(manipulator.GetArmIndices())
    ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot, iktype=IkParameterization.Type.Transform6D,
                                                                 forceikfast=True, freeindices=None, freejoints=None, manip=None)
    if not ikmodel.load():
        print 'Generating inverse kinematics model'
        ikmodel.autogenerate()

    base_manip = interfaces.BaseManipulation(
        robot, plannername=None, maxvelmult=None)
    ir_model = databases.inversereachability.InverseReachabilityModel(robot)
    if ir_model.load():
        print 'Generating inverse reachability model'
        ir_model.autogenerate()

    return robot, manipulator, base_manip, ir_model def execute_viewer(env, execute):
    if sys.platform == 'darwin':
        SetViewerUserThread(env, 'qtcoin', execute)
    else:
        env.SetViewer('qtcoin')
        execute() 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 MultiEasyGenStream(EasyGenStream):

    class StreamFn(GeneratorStream):

        def get_generator(self, inputs):
            values = tuple(inp.type.get_value(inp) for inp in inputs)

            for output_list in self.cond_stream.generator(*values):
                assert type(output_list) == list
                new_output_list = []
                for outputs in output_list:
                    if type(outputs) not in [list, tuple]:
                        outputs = [outputs]
                    assert len(outputs) == len(self.cond_stream.outputs)
                    new_output_list.append(
                        tuple(par.type(out) for par, out in zip(self.cond_stream.outputs, outputs)))

                yield new_output_list 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 SEPARATOR = '\n' + 85 * '-' + '\n' INF = float('inf') def cfree_pose_fn(env, body1, body2):
    def cfree_pose(pose1, pose2):
        body1.Enable(True)
        set_pose(body1, pose1.value)
        body2.Enable(True)
        set_pose(body2, pose2.value)
        return not env.CheckCollision(body1, body2)
    return cfree_pose def cfree_traj_fn(env, manipulator, body1, body2, all_bodies):
    robot = manipulator.GetRobot()

    def _cfree_traj_pose(traj, pose):
        enable_all(all_bodies, False)
        body2.Enable(True)
        set_pose(body2, pose.value)
        for conf in traj.value:
            set_manipulator_conf(manipulator, conf)
            if env.CheckCollision(robot, body2):
                return False
        return True

    def _cfree_traj_grasp_pose(traj, grasp, pose):
        enable_all(all_bodies, False)
        body1.Enable(True)
        body2.Enable(True)
        set_pose(body2, pose.value)
        for conf in traj.value:
            set_manipulator_conf(manipulator, conf)
            manip_trans = manipulator.GetTransform()
            set_pose(body1, object_trans_from_manip_trans(
                manip_trans, grasp.value))
            if env.CheckCollision(body1, body2):
                return False
        return True

    def cfree_traj(traj, pose):
        if DISABLE_MOTION_COLLISIONS:
            return True
        if traj.pose is not None and traj.pose == pose:

            return True
        return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose))
    return cfree_traj def sample_grasp_traj_fn(env, manipulator, body1, all_bodies):
    robot = manipulator.GetRobot()

    def sample_grasp_traj(pose, grasp):
        enable_all(all_bodies, False)
        body1.Enable(True)
        set_pose(body1, pose.value)
        manip_trans = manip_from_pose_grasp(pose.value, grasp.value)
        grasp_conf = solve_inverse_kinematics(manipulator, manip_trans)
        if grasp_conf is None:
            return
        if DISABLE_MOTIONS:
            yield [(Conf(grasp_conf), Traj([]))]
            return

        set_manipulator_conf(manipulator, grasp_conf)
        robot.Grab(body1)

        pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR))
        pregrasp_conf = solve_inverse_kinematics(manipulator, pregrasp_trans)
        if pregrasp_conf is None:
            return

        if has_mp():
            path = mp_straight_line(robot, grasp_conf, pregrasp_conf)
        else:
            path = linear_motion_plan(robot, pregrasp_conf)

        robot.Release(body1)
        if path is None:
            return
        grasp_traj = Traj(path)
        grasp_traj.pose = pose
        grasp_traj.grasp = grasp
        yield [(Conf(pregrasp_conf), grasp_traj)]
    return sample_grasp_traj def sample_free_motion_fn(manipulator, base_manip, all_bodies):
    robot = manipulator.GetRobot()

    def sample_free_motion(conf1, conf2):
        if DISABLE_MOTIONS:

            traj = Traj([conf2.value])
            traj.pose = None
            traj.grasp = None
            yield [(traj,)]
            return
        enable_all(all_bodies, False)
        set_manipulator_conf(manipulator, conf1.value)

        if has_mp():
            path = mp_birrt(robot, conf1.value, conf2.value)
        else:

            path = manipulator_motion_plan(
                base_manip, manipulator, conf2.value, max_iterations=10)

        if path is None:
            return
        traj = Traj(path)
        traj.pose = None
        traj.grasp = None
        yield [(traj,)]
    return sample_free_motion def sample_holding_motion_fn(manipulator, base_manip, body1, all_bodies):
    robot = manipulator.GetRobot()

    def sample_holding_motion(conf1, conf2, grasp):
        if DISABLE_MOTIONS:

            traj = Traj([conf2.value])
            traj.pose = None
            traj.grasp = grasp
            yield [(traj,)]
            return
        enable_all(all_bodies, False)
        body1.Enable(True)
        set_manipulator_conf(manipulator, conf1.value)
        manip_trans = manipulator.GetTransform()
        set_pose(body1, object_trans_from_manip_trans(
            manip_trans, grasp.value))
        robot.Grab(body1)

        if has_mp():
            path = mp_birrt(robot, conf1.value, conf2.value)
        else:

            path = manipulator_motion_plan(
                base_manip, manipulator, conf2.value, max_iterations=10)

        robot.Release(body1)
        if path is None:
            return
        traj = Traj(path)
        traj.pose = None
        traj.grasp = grasp
        yield [(traj,)]
    return sample_holding_motion def visualize_solution(env, problem, initial_conf, robot, manipulator, bodies, plan):
    def _execute_traj(confs):
        for j, conf in enumerate(confs):
            set_manipulator_conf(manipulator, conf)
            sleep(0.05)

    set_manipulator_conf(manipulator, initial_conf.value)
    for obj, pose in problem.initial_poses.iteritems():
        set_pose(bodies[obj], pose.value)

    raw_input('Start?')
    for i, (action, args) in enumerate(plan):

        if action.name == 'move':
            _, _, traj = args
            _execute_traj(traj.value)
        elif action.name == 'move_holding':
            _, _, traj, _, _ = args
            _execute_traj(traj.value)
        elif action.name == 'pick':
            obj, _, _, _, traj = args
            _execute_traj(traj.value[::-1])
            robot.Grab(bodies[obj])
            _execute_traj(traj.value)
        elif action.name == 'place':
            obj, _, _, _, traj = args
            _execute_traj(traj.value[::-1])
            robot.Release(bodies[obj])
            _execute_traj(traj.value)
        else:
            raise ValueError(action.name)
        env.UpdatePublishedBodies()
import argparse
import sys
from openravepy import Environment, RaveDestroy
from stripstream.algorithms.focused.simple_focused import simple_focused
from stripstream.algorithms.search.fast_downward import get_fast_downward
from stripstream.pddl.utils import convert_plan
from robotics.openrave.problems import dantam_distract
from robotics.openrave.utils import open_gripper, \
    Conf, initialize_openrave, execute_viewer
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 MultiEasyGenStream, EasyTestStream
from stripstream.utils import SEPARATOR, INF
from robotics.openrave.tamp_fixed_base import cfree_pose_fn, cfree_traj_fn, \
    sample_grasp_traj_fn, sample_free_motion_fn, sample_holding_motion_fn, visualize_solution
#!/usr/bin/env python2



#from stripstream.algorithms.incremental.incremental_planner import incremental_planner



Type, Pred, Param = EasyType, EasyPredicate, EasyParameter

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

PROBLEM = lambda env: dantam_distract(env, 8)
ARM = 'leftarm'
MAX_GRASPS = 1

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

# Types
OBJ, POSE, GRASP = Type(), Type(), Type()
CONF, TRAJ = Type(), Type()

# Fluents
ConfEq = Pred(CONF)
PoseEq = Pred(OBJ, POSE)
GraspEq = Pred(OBJ, GRASP)
HandEmpty = Pred()

# Derived
SafePose = Pred(OBJ, POSE)
SafeTraj = Pred(OBJ, TRAJ)

# Static trajectory
FreeMotion = Pred(CONF, CONF, TRAJ)
HoldingMotion = Pred(CONF, CONF, GRASP, TRAJ)
GraspMotion = Pred(POSE, GRASP, CONF, TRAJ)

# Static collision
CFreePose = Pred(POSE, POSE)
CFreeTraj = Pred(TRAJ, POSE)

O, P, G = Param(OBJ), Param(POSE), Param(GRASP)
O2, P2 = Param(OBJ), Param(POSE)
Q, Q2 = Param(CONF), Param(CONF)
T = Param(TRAJ)

rename_easy(locals())

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

actions = [
    Action(name='pick', parameters=[O, P, G, Q, T],
           condition=And(PoseEq(O, P), HandEmpty(), ConfEq(Q), GraspMotion(P, G, Q, T),
                         ForAll([O2], Or(Equal(O, O2), SafeTraj(O2, T)))),
           # ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2,
           # GT))))),
           effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),

    Action(name='place', parameters=[O, P, G, Q, T],
           condition=And(GraspEq(O, G), ConfEq(Q), GraspMotion(P, G, Q, T),
                         ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeTraj(O2, T))))),
           effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),

    Action(name='move', parameters=[Q, Q2, T],
           condition=And(ConfEq(Q), HandEmpty(), FreeMotion(Q, Q2, T),
                         ForAll([O2], SafeTraj(O2, T))),
           effect=And(ConfEq(Q2), Not(ConfEq(Q)))),

    Action(name='move_holding', parameters=[Q, Q2, T, O, G],
           condition=And(ConfEq(Q), GraspEq(O, G), HoldingMotion(Q, Q2, G, T),
                         ForAll([O2], Or(Equal(O, O2), SafeTraj(O2, T)))),
           effect=And(ConfEq(Q2), Not(ConfEq(Q)))),
]

axioms = [
    Axiom(effect=SafePose(O2, P), condition=Exists(
        [P2], And(PoseEq(O2, P2), CFreePose(P, P2)))),
    Axiom(effect=SafeTraj(O2, T), condition=Exists(
        [P2], And(PoseEq(O2, P2), CFreeTraj(T, P2)))),
]

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


def solve_tamp(env):
    viewer = env.GetViewer() is not None
    problem = PROBLEM(env)

    robot, manipulator, base_manip, _ = initialize_openrave(
        env, ARM, min_delta=.01)
    bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
    all_bodies = bodies.values()
    # Assuming all objects has the same geometry
    assert len({body.GetKinematicsGeometryHash() for body in all_bodies}) == 1
    body1 = all_bodies[-1]  # Generic object 1
    body2 = all_bodies[-2] if len(bodies) >= 2 else body1  # Generic object 2
    grasps = problem.known_grasps[:MAX_GRASPS] if problem.known_grasps else []
    poses = problem.known_poses if problem.known_poses else []

    open_gripper(manipulator)
    initial_conf = Conf(robot.GetConfigurationValues()
                        [manipulator.GetArmIndices()])

    cfree_pose = cfree_pose_fn(env, body1, body2)
    cfree_traj = cfree_traj_fn(env, manipulator, body1, body2, all_bodies)
    sample_grasp_traj = sample_grasp_traj_fn(
        env, manipulator, body1, all_bodies)

    sample_free_motion = sample_free_motion_fn(