コード例 #1
0
def strips_planner(universe, search, max_time=INF, max_cost=INF, verbose=False):
  initial_atoms = filter(lambda atom: not isinstance(atom, Initialize), universe.get_initial_atoms())
  objects = universe.type_to_objects
  problem = STRIPStreamProblem(initial_atoms, universe.goal_formula, universe.actions + universe.axioms, []) # TODO - constants?
  static_map = {pr: [] for pr in problem.get_static_predicates()}
  for atom in initial_atoms:
    if atom.predicate in static_map:
      static_map[atom.predicate].append(atom)

  initial_state = State(PyLiteral(atom) for atom in initial_atoms if atom.predicate not in static_map)
  dnf_goal = list(universe.goal_formula.propositional(objects).get_literals())
  if not dnf_goal:
    return []
  if len(dnf_goal) != 1:
    raise NotImplementedError('Currently only support a conjunctive goal: %s'%universe.goal_formula)
  goal_state = PartialState(map(PyLiteral, dnf_goal[0]))
  strips_actions = list(get_actions(universe.actions, static_map, objects))
  strips_axioms = list(get_axioms(universe.axioms, static_map, objects))
  if verbose:
    print 'Actions:', len(strips_actions)
    print 'Axioms:', len(strips_axioms)

  plan, state_space = default_derived_plan(initial_state, goal_state, strips_actions, strips_axioms)
  if verbose:
    print plan
    print state_space
  if plan is None:
    return None
  return [(universe.name_to_action[action.lifted.name], action.args) for action in plan.operators]
コード例 #2
0
def compile_problem(oracle):
    problem = oracle.problem
    CSpace.robot_base(oracle.robot).set_active()

    initial_config = Config(
        base_values_from_full_config(oracle.initial_config.value))
    initial_atoms = [
        AtConfig(Conf(initial_config)),
    ]
    print 'Initial', initial_config.value

    goal_literals = [AtConfig(Conf(problem.goal_config))
                     ] if problem.goal_config is not None else []
    print 'Goal', problem.goal_config.value

    actions = [
        Move(oracle),
    ]

    axioms = [
        #InRegionAxiom(),
    ]

    cond_streams = [
        CollisionFreeTest(oracle),
        ConfStream(oracle),
    ]

    objects = []

    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
                              cond_streams, objects)
コード例 #3
0
def constraint_to_stripstream(fts_problem, do_axioms=True, test_streams=True):
    expanded_state_vars = expand_variables(fts_problem.state_vars)
    #expanded_control_vars = expand_variables(fts_problem.control_vars) # TODO - use this to check variables

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

    actions, axioms = convert_transition(fts_problem.transition, var_map,
                                         axiom_map)
    cond_streams = map(
        convert_sampler,
        fts_problem.samplers)  # TODO - prune not useful samplers

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

    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
                              cond_streams, objects)
コード例 #4
0
def observable_problem(belief, goal, costs=True): # NOTE - costs is really important here
  #objects = belief.objLoc.keys()
  locations = belief.occupancies.keys()
  states = [dirty, clean, dry, wet]

  initial_atoms = []
  occupied = set()
  for obj in belief.objLoc.keys():
    dist = belief.objLocDist(obj)
    loc, p = dist.computeMPE() # TODO - concentration inequalities for Gaussian
    if p >= LOC_CONFIDENCE:
      initial_atoms.append(At(obj, loc))
      occupied.add(loc)
    else:
      initial_atoms.append(UnsureLoc(obj))
    for loc in locations:
      p = dist.prob(loc)
      cost = int(COST_SCALE*1./max(p, MIN_P)) if costs else COST_SCALE*1 # TODO - set to infinity if too large (equivalent to blocking)
      initial_atoms.append(Initialize(FindCost(OBJ(obj), LOC(loc)), cost))
      #if p < MIN_CONFIDENCE:
      #  initial_atoms.append(NotAtLoc(obj, loc))

  for obj in belief.objState.keys():
    dist = belief.objState[obj]
    state, p = dist.computeMPE()
    if p >= STATE_CONFIDENCE:
      initial_atoms.append(HasState(obj, state))
    else:
      initial_atoms.append(UnsureState(obj))
    for state in states:
      p = dist.prob(state)
      cost = int(COST_SCALE*1./max(p, MIN_P)) if costs else COST_SCALE*1
      initial_atoms.append(Initialize(InspectStateCost(OBJ(obj), STATE(state)), cost))
      #if p < MIN_CONFIDENCE:
      #  initial_atoms.append(NotHasState(obj, loc))

  for loc in belief.occupancies.keys():
    if loc == 'washer':
      initial_atoms.append(IsWasher(loc))
    elif loc == 'painter':
      initial_atoms.append(IsPainter(loc))
    elif loc == 'dryer':
      initial_atoms.append(IsDryer(loc))

  goal_literals = []
  for fluent in goal.fluents:
    if isinstance(fluent, Bd):
      literal, arg, prob = fluent.args
      if isinstance(literal, ObjLoc):
        goal_literals.append(At(literal.args[0], literal.value))
      elif isinstance(literal, ObjState):
        goal_literals.append(HasState(literal.args[0], arg))
      elif isinstance(literal, toyTest.Clear):
        goal_literals.append(Clear(literal.args[0]))
      else:
        raise NotImplementedError(literal)
    else:
      raise NotImplementedError(fluent)

  return STRIPStreamProblem(initial_atoms, goal_literals, actions, [], [])
コード例 #5
0
def get_problem():
    region = [1, 2, 3]

    #mergeable_types = {BLOCK}

    block = C('block%s' % 1, BLOCK)
    obstacle = C('block%s' % 2, BLOCK)
    blocks = [block]
    #blocks = [block, obstacle]

    pose = Pose(10)
    config = Config(0)
    region = C('region%s' % 1, REGION)

    initial_atoms = [
        AtConfig(config),
        HandEmpty(),
        AtPose(block, pose),
        #AtPose(obstacle, Pose(20)),
        #Unsafe(pose),
    ] + [
        #AtPose(obj, pose) for obj, pose in object_poses.iteritems()
    ]

    goal_literals = [
        Holding(block),
        #Not(HandEmpty(robot)), # NOTE - negative goal
        #Not(AtPose(obj, Pose(0))),
        #AtPose(block, Pose(20)),
        #AtPose(obj, Pose(4)),
        #AtConfig(Config(5)),
        #AtConfig(robot, Config(4)),
        #InRegion(block, region),
    ]

    operators = [
        Move(),
        Pick(),
        Place(blocks),
        #InRegionAxiom(),
    ]
    if COLLISIONS:
        operators.append(SafeAxiom())

    cond_streams = [
        PoseStream(),
        #IKStream(),
        CollisionFreeTest(),
        ConfigStream(),
        #IKStreamBad(),
        IKTest(),
    ]

    objects = [  # Assorted objects
    ]

    return STRIPStreamProblem(initial_atoms, goal_literals, operators,
                              cond_streams, objects)
コード例 #6
0
def create_problem():
    """
    Creates a blocksworld STRIPStream problem.

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

    # Data types
    BLOCK = Type()

    # Fluent predicates
    Clear = Pred(BLOCK)
    OnTable = Pred(BLOCK)
    ArmEmpty = Pred()
    Holding = Pred(BLOCK)
    On = Pred(BLOCK, BLOCK)

    # Free parameters
    B1, B2 = Param(BLOCK), Param(BLOCK)

    rename_easy(locals())

    actions = [
        Action(name='pickup', parameters=[B1],
               condition=And(Clear(B1), OnTable(B1), ArmEmpty()),
               effect=And(Holding(B1), Not(Clear(B1)), Not(OnTable(B1)), Not(ArmEmpty()))),

        Action(name='putdown', parameters=[B1],
               condition=Holding(B1),
               effect=And(Clear(B1), OnTable(B1), ArmEmpty(), Not(Holding(B1)))),

        Action(name='stack', parameters=[B1, B2],
               condition=And(Clear(B2), Holding(B1)),
               effect=And(Clear(B1), On(B1, B2), ArmEmpty(), Not(Clear(B2)), Not(Holding(B1)))),

        Action(name='unstack', parameters=[B1, B2],
               condition=And(Clear(B1), On(B1, B2), ArmEmpty()),
               effect=And(Clear(B2), Holding(B1), Not(Clear(B1)), Not(On(B1, B2)), Not(ArmEmpty()))),
    ]

    axioms = []
    cond_streams = []
    constants = []

    initial_atoms = [
        OnTable('A'),
        OnTable('B'),
        OnTable('C'),
        Clear('A'),
        Clear('B'),
        Clear('C'),
        ArmEmpty(),
    ]

    goal_literals = [On('A', 'B'), On('B', 'C')]

    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
コード例 #7
0
def compile_problem(tamp_problem):
    blocks = [BlockO(block) for block in tamp_problem.get_blocks()]

    initial_atoms = [
        AtConfig(ConfigO(tamp_problem.initial_config)),
    ] + [
        AtPose(BlockO(block), PoseO(pose))
        for block, pose in tamp_problem.initial_poses
    ] + [
        IsPose(BlockO(block), PoseO(pose))
        for block, pose in tamp_problem.initial_poses
    ]
    if tamp_problem.initial_holding is None:
        initial_atoms.append(HandEmpty())
    else:
        initial_atoms.append(Holding(BlockO(tamp_problem.initial_holding)))

    goal_literals = []
    if tamp_problem.goal_config is not None:
        goal_literals.append(AtConfig(ConfigO(tamp_problem.goal_config)))
    if tamp_problem.goal_holding is False:
        goal_literals.append(HandEmpty())
    elif tamp_problem.goal_holding is not None:
        goal_literals.append(Holding(BlockO(tamp_problem.goal_holding)))
    for block, goal in tamp_problem.goal_poses:
        goal_literals.append(AtPose(BlockO(block), PoseO(goal)))
    for block, goal in tamp_problem.goal_regions:
        goal_literals.append(InRegion(BlockO(block), RegionO(goal)))

    operators = [
        Pick(),
        Place(),
        InRegionAxiom(),
    ]

    cond_streams = [
        GraspStream(tamp_problem.robot),
        RegionStream(),
        RegionTest(),
        IKStream(tamp_problem),
    ]

    if COLLISIONS:
        operators.append(SafeAxiom())
        cond_streams.append(CollisionFreeTest())
    if USE_BASE:
        operators.append(Move(blocks))
        operators.append(MoveHolding(blocks))
        cond_streams.append(MotionStream(tamp_problem))

    objects = [  # Assorted objects
        RegionO(tamp_problem.env_region),
    ]

    return STRIPStreamProblem(initial_atoms, goal_literals, operators,
                              cond_streams, objects)
コード例 #8
0
def observable_problem(env, start, goal):
    locations = start.details.occupancies.keys()

    initial_atoms = []
    constants = []
    occupied = set()
    for obj in env.objects:
        for attr in env.objects[obj]:
            if attr == 'clean':
                initial_atoms.append(Clean(obj))
            elif attr == 'dirty':
                pass
            elif attr in locations:
                initial_atoms.append(At(obj, attr))
                occupied.add(attr)
            else:
                raise NotImplementedError(attr)
    for loc in locations:
        if loc not in occupied:
            initial_atoms.append(Clear(loc))
        constants.append(LOC(loc))
        if loc == 'washer':
            initial_atoms.append(IsWasher(loc))
        if loc == 'painter':
            initial_atoms.append(IsPainter(loc))
        if loc == 'dryer':
            initial_atoms.append(IsDryer(loc))

    goal_literals = []
    for fluent in goal.fluents:
        if isinstance(fluent, Bd):
            literal, arg, prob = fluent.args
            if isinstance(literal, ObjLoc):
                goal_literals.append(At(literal.args[0], literal.value))
            elif isinstance(literal, ObjState) and arg == 'clean':
                goal_literals.append(Clean(literal.args[0]))
            elif isinstance(literal, ObjState) and arg == 'wetPaint':
                goal_literals.append(WetPaint(literal.args[0]))
            elif isinstance(literal, ObjState) and arg == 'dryPaint':
                goal_literals.append(DryPaint(literal.args[0]))
            else:
                raise NotImplementedError(literal)
        else:
            raise NotImplementedError(fluent)

    return STRIPStreamProblem(initial_atoms, goal_literals, actions, [],
                              constants)
コード例 #9
0
def compile_belief(belief, goal):
    constants = map(OBJ, belief.objLoc.keys()) + map(LOC,
                                                     belief.occupancies.keys())
    #constants = []
    initial_atoms = [UnknownAt(obj) for obj in belief.objLoc]
    initial_atoms += [
        BAt(obj, belief.objLocDist(obj)) for obj in belief.objLoc
    ]  # NOTE - objLocDist != objLoc
    goal_literals = []
    for fluent in goal.fluents:
        if isinstance(fluent, Bd):
            literal, arg, prob = fluent.args
            if isinstance(literal, ObjLoc):
                goal_literals.append(
                    BAtAbove(literal.args[0], literal.value, prob))
            #elif isinstance(literal, ObjState) and arg == 'clean':
            #  goal_literals.append(BClean(literal.args[0], prob))
            else:
                raise NotImplementedError(literal)
        else:
            raise NotImplementedError(fluent)

    cond_streams = [
        # Forward streams
        #LookUpdate(),
        #PerfectLookUpdate(),

        # Oriented streams
        #LookPlan(),
        #LookJump(),
        PerfectLookJump(),
        #PerfectLookJump2(),

        # Independent costs
        LookCostFn(),
        MoveCostFn(),
        MoveUpdate(),
        #GeneratorStream(inputs=[L, B, L2], outputs=[B2], conditions=[], effects=[IsMoveUpdate(L, B, L2, B2)],
        #                generator=lambda l, b, l2: [move_update(l, b, l2)[0]]),

        #TestStream(inputs=[B, L, P], conditions=[], effects=[BSatisfies(B, L, P)],
        #           test=is_above, eager=True),
    ]

    return STRIPStreamProblem(initial_atoms, goal_literals, actions,
                              cond_streams, constants)
コード例 #10
0
def get_problem():
    satellite = C('satellite1', SATELLITE)
    satellite_pos = 50

    initial_atoms = [
        #AtState(Position(0), Velocity(0)),
        AtState(State((0, 0))),
        Carrying(satellite),
        ArePair(State((satellite_pos, 0)), Position(satellite_pos)),
        Landed(),
    ]

    goal_literals = [
        #Above(Position(20)),
        #AtState(State((50, 0))),
        AtOrbit(satellite, Position(satellite_pos)),
        Landed(),
    ]

    operators = [
        Burst(),
        Glide(),
        Deploy(),
        TakeOff(),
        Land(),
        AboveAxiom(),
    ]

    cond_streams = [
        ForwardBurstStream(
        ),  # TODO - BackwardBurstStream in order to move back from the goal
        #FixedBurstStream(),
        TargetBurstStream(),
        ForwardGlideStream(),
        AboveTest(),
    ]

    objects = [  # Assorted objects
        #Acceleration(ACCELERATION_RANGE[1]),
        #Acceleration(ACCELERATION_RANGE[0]),
        #Acceleration(0),
    ]

    return STRIPStreamProblem(initial_atoms, goal_literals, operators,
                              cond_streams, objects)
コード例 #11
0
def create_abstract_problem(state, goal):
    # Goal serialization is basically like this
    # Necessary and sufficient?

    tables, initial_poses, initial_config, holding = state

    # TODO: connect this to goals on the actual states
    # The quantification of poses and grasps is independent
    # Conditions are derived or atoms
    actions = [
        Action(
            name='pick',
            parameters=[B, S],
            condition=And(On(B, S), HandEmpty()),  #, AtConf(Q)),
            #condition=And(On(B, S), HandEmpty(), Not(Holding(B))),  # , AtConf(Q)),
            effect=And(Holding(B), Not(On(B, S)), Not(HandEmpty()))),
        Action(
            name='place',
            parameters=[B, S],
            condition=And(Holding(B)),  #, AtConf(Q)),
            effect=And(On(B, S), HandEmpty(), Not(Holding(B)))),
        #Action(name='move', parameters=[S1, Q2],
        #  condition=AtConf(Q),
        #  effect=And(AtConf(Q2), Not(AtConf(Q)))),
    ]

    axioms = []
    cond_streams = []

    initial_atoms = [
        #AtConf(initial_config),
        HandEmpty()
    ] + [On(block, pose.sur) for block, pose in initial_poses.items()]

    goal_literals = [On(b, s) for b, s in goal.on.items()]
    if goal.holding is not None:
        goal_literals.append(Holding(goal.holding))

    problem = STRIPStreamProblem(initial_atoms, goal_literals,
                                 actions + axioms, cond_streams)

    return problem
コード例 #12
0
def compile_problem(tamp_problem, stream_fn=generative_streams):
    blocks = [C(block, BLOCK) for block in tamp_problem.get_blocks()]

    initial_atoms = [
        AtConfig(Config(tamp_problem.initial_config)),
    ] + [
        AtPose(C(block, BLOCK), Pose(pose))
        for block, pose in tamp_problem.initial_poses.iteritems()
    ]
    if tamp_problem.initial_holding is False:
        initial_atoms.append(HandEmpty())
    else:
        initial_atoms.append(Holding(C(tamp_problem.initial_holding, BLOCK)))

    goal_literals = []
    if tamp_problem.goal_holding is False:
        goal_literals.append(HandEmpty())
    elif tamp_problem.goal_holding is not None:
        goal_literals.append(Holding(C(tamp_problem.goal_holding, BLOCK)))
    for block, goal in tamp_problem.goal_poses.iteritems():
        goal_literals.append(AtPose(C(block, BLOCK), Pose(goal)))

    operators = [
        Move(),
        #H1Pick(),
        Pick(),
        Place(blocks),
        #InRegionAxiom(),
    ]
    if COLLISIONS:
        operators.append(SafeAxiom())

    objects = [  # Assorted objects
    ]

    return STRIPStreamProblem(initial_atoms, goal_literals, operators,
                              stream_fn(), objects)
コード例 #13
0
def create_problem(goal, obstacles=(), distance=.25, digits=3):
    """
  Creates a Probabilistic Roadmap (PRM) motion planning problem.

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

    # Data types
    POINT = Type()
    REGION = Type()

    # Fluent predicates
    AtPoint = Pred(POINT)

    # Derived predicates
    InRegion = Pred(REGION)
    #IsReachable = Pred(POINT, POINT)
    IsReachable = Pred(POINT)

    # Stream predicates
    IsEdge = Pred(POINT, POINT)
    Contained = Pred(POINT, REGION)

    # Free parameters
    P1, P2 = Param(POINT), Param(POINT)
    R = Param(REGION)

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

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

    actions = [
        Action(name='move',
               parameters=[P1, P2],
               condition=And(AtPoint(P1), IsReachable(P2)),
               effect=And(AtPoint(P2), Not(AtPoint(P1))))
    ]

    axioms = [
        Axiom(effect=InRegion(R),
              condition=Exists([P1], And(AtPoint(P1), Contained(P1, R)))),
        Axiom(effect=IsReachable(P2),
              condition=Or(AtPoint(P2),
                           Exists([P1], And(IsReachable(P1), IsEdge(P1,
                                                                    P2))))),
    ]

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

    def sampler():
        for _ in inf_sequence():
            yield [(sample(digits), ) for _ in range(10)]

    roadmap = set()

    def test(p1, p2):
        if not (get_distance(p1, p2) <= distance and is_collision_free(
            (p1, p2), obstacles)):
            return False
        roadmap.add((p1, p2))
        return True

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

    # Conditional stream declarations
    cond_streams = [
        EasyListGenStream(
            inputs=[],
            outputs=[P1],
            conditions=[],
            effects=[],
            generator=sampler
        ),  # NOTE - version that only generators collision-free points
        GeneratorStream(inputs=[R],
                        outputs=[P1],
                        conditions=[],
                        effects=[Contained(P1, R)],
                        generator=lambda r:
                        (sample_box(r) for _ in inf_sequence())),
        TestStream(inputs=[P1, P2],
                   conditions=[],
                   effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
                   test=test,
                   eager=True),
    ]

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

    constants = []

    initial_atoms = [
        AtPoint((0, 0)),
    ]

    goal_literals = []
    if is_region(goal):
        goal_literals.append(InRegion(goal))
    else:
        goal_literals.append(AtPoint(goal))

    problem = STRIPStreamProblem(initial_atoms, goal_literals,
                                 actions + axioms, cond_streams, constants)

    return problem, roadmap
コード例 #14
0
def compile_problem(oracle):
    problem = oracle.problem
    for obj in problem.goal_poses:
        if problem.goal_poses[obj] == 'initial':
            problem.goal_poses[obj] = oracle.initial_poses[obj]
        elif problem.goal_poses[
                obj] in oracle.initial_poses:  # Goal names other object (TODO - compile with initial)
            problem.goal_poses[obj] = oracle.initial_poses[
                problem.goal_poses[obj]]
    # oracle.initial_config = ??? # TODO - set the initial configuration

    transit_conf = Config(oracle.default_left_arm_config)

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

    O, P, G, T = Param(OBJ), Param(POSE), Param(GRASP), Param(TRAJ)
    OB, R = Param(OBJ), Param(REG)

    BQ, MQ = Param(BASE_CONF), Param(MANIP_CONF)
    BQ2, MQ2 = Param(BASE_CONF), Param(MANIP_CONF)
    BT, MT = Param(BASE_TRAJ), Param(MANIP_TRAJ)

    rename_easy(locals())

    # Separate arm and base configs and trajectories?
    # Make a fixed configuration for arm stuff

    # If I separate arm and base poses, I can avoid worrying about collision when the base and things

    # Would I ever want to actually separate these and use the same grasp at a different config?
    # - Maybe for two arms?
    # - I don't want to reuse trajectories at different base configs. That wouldn't make sense
    # - Although if I manipulate two configs at once, maybe it would!

    # - Make constant arm transit configs?

    # TODO - maybe make an axiom to handle BT and MT when doing these things?
    # TODO - I could equip a manip conf with a base conf at all times if I really don't want them separate
    # NOTE - put then would need to update them whenever moving the base

    #actions = [
    #   Action(name='pick', parameters=[O, P, G, BQ, MQ],
    #     condition=And(PoseEq(O, P), HandEmpty(), Kin(O, P, G, BQ, MQ), BaseEq(BQ), ManipEq(MQ)),
    #       #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
    #     effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
    #
    #   Action(name='place', parameters=[O, P, G, BQ, MQ],
    #     condition=And(GraspEq(O, G), Kin(O, P, G, BQ, MQ), BaseEq(BQ), ManipEq(MQ)),
    #       #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
    #     effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
    #
    #   Action(name='move_arm', parameters=[MQ, MQ2, BQ, MT],
    #     condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT)),
    #     effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
    #
    #   Action(name='move_base', parameters=[BQ, BQ2, MQ, BT], # TODO - put the arm motion stuff in an axiom
    #     condition=And(BaseEq(BQ), TransitManip(MQ), BaseMotion(BQ, BQ2, BT)),
    #     effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))),
    # ]

    # TODO - should I include the grasp in move backwards trajectory?

    actions = [
        abs_action(
            name='pick',
            parameters=[O, P, G, BQ, MQ],
            conditions=[
                And(PoseEq(O, P), HandEmpty(),
                    Kin(O, P, G, BQ,
                        MQ)),  #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
                And(BaseEq(BQ), ManipEq(MQ))
            ],
            effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
        abs_action(
            name='place',
            parameters=[O, P, G, BQ, MQ],
            conditions=[
                And(GraspEq(O, G),
                    Kin(O, P, G, BQ,
                        MQ)),  #ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
                And(BaseEq(BQ), ManipEq(MQ))
            ],
            effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),

        # NOTE - the hierarchical versions of this
        abs_action(
            name='move_arm',
            parameters=[MQ, MQ2, BQ,
                        MT],  # TODO - Or(TransitManip(MQ), TransitManip(MQ2))
            conditions=[
                And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ)),
                #And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), Or(TransitManip(MQ), TransitManip(MQ2))) # This does something odd
                #And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ),
                #  Or(TransitManip(MQ), LegalConf(BQ, MQ)),
                #  Or(TransitManip(MQ2), LegalConf(BQ, MQ2))) # This does something odd
            ],  # TODO - put an Or(Pair(MQ, BQ), Pair(MQ2, BQ)) or something
            effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
        abs_action(name='move_base',
                   parameters=[BQ, BQ2, BT],
                   conditions=[
                       And(BaseEq(BQ), SafeManip(BT), BaseMotion(BQ, BQ2, BT))
                   ],
                   effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))),

        #abs_action(name='move_base', parameters=[BQ, BQ2, MQ, BT],
        #  conditions=[
        #    And(BaseEq(BQ), TransitManip(MQ), BaseMotion(BQ, BQ2, BT))],
        #  effect=And(BaseEq(BQ2), Not(BaseEq(BQ)))),
    ]
    # NOTE - the parameters really aren't necessary

    # TODO - could remove any dependence on tests because there are so cheap with the evaluation (already can't use them with axioms)
    # TODO - could also just make the cost only depend on the introduced objects within the effects

    axioms = [
        Axiom(effect=Holding(O),
              condition=Exists([G], And(GraspEq(O, G), LegalGrasp(O, G)))),
        Axiom(effect=InRegion(O, R),
              condition=Exists([P], And(PoseEq(O, P), Contained(R, O, P)))),

        #Axiom(effect=Safe(O, T), condition=Exists([P], And(PoseEq(O, P), CFree(O, P, T)))),

        #Axiom(effect=SafeArm(O, MQ, T), condition=Exists([P, MQ], And(PoseEq(O, P), ManipEq(MQ), CFree(O, P, T)))),
        Axiom(effect=SafeManip(BT),
              condition=Exists(
                  [MQ], And(ManipEq(MQ),
                            TransitManip(MQ)))),  # TODO - add condition here
    ]

    # TODO - include parameters in STRIPS axiom?

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

    def get_base_conf(conf):
        return Config(base_values_from_full_config(conf.value))

    def get_arm_conf(conf):
        return Config(
            arm_from_full_config(oracle.robot.GetActiveManipulator(),
                                 conf.value))

    def sample_poses(o, num_samples=1):
        while True:
            if AVOID_INITIAL:
                oracle.set_all_object_poses(oracle.initial_poses)
            else:
                oracle.set_all_object_poses({o: oracle.initial_poses[o]})
            yield list(
                islice(
                    random_region_placements(oracle,
                                             o,
                                             oracle.get_counters(),
                                             region_weights=True),
                    num_samples))

    def sample_grasps(o):
        yield get_grasps(oracle, o)

    def sample_region(o, r, num_samples=1):
        while True:
            oracle.set_all_object_poses({o: oracle.initial_poses[o]})
            yield list(
                islice(
                    random_region_placements(oracle,
                                             o, [r],
                                             region_weights=True),
                    num_samples))

    def sample_motion(o, p, g, max_calls=1, max_failures=50):
        oracle.set_all_object_poses(
            {o: p})  # TODO - saver for the initial state as well?
        if oracle.approach_collision(o, p, g):
            return
        for i in range(max_calls):
            pap = PickAndPlace(oracle.get_geom_hash(o), p, g)
            if not pap.sample(oracle,
                              o,
                              max_failures=max_failures,
                              sample_vector=False,
                              sample_arm=False,
                              check_base=False):
                break
            #pap.obj = o
            yield [(get_base_conf(pap.grasp_config),
                    get_arm_conf(pap.grasp_config))]

    def collision_free(o, p, t):
        if p is None or o == t.obj:
            return True
        holding = ObjGrasp(t.obj, t.grasp)
        #holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
        if not DO_ARM_MOTION:
            return not oracle.holding_collision(t.grasp_config, o, p, holding)
        return not oracle.traj_holding_collision(t.approach_config, t.trajs, o,
                                                 p, holding)

    cond_streams = [
        EasyListGenStream(inputs=[O],
                          outputs=[P],
                          conditions=[],
                          effects=[LegalPose(O, P)],
                          generator=sample_poses),
        EasyListGenStream(inputs=[O],
                          outputs=[G],
                          conditions=[],
                          effects=[LegalGrasp(O, G)],
                          generator=sample_grasps),
        EasyListGenStream(inputs=[O, R],
                          outputs=[P],
                          conditions=[],
                          effects=[LegalPose(O, P),
                                   Contained(R, O, P)],
                          generator=sample_region),

        #MultiEasyGenStream(inputs=[O, P, G], outputs=[BQ, MQ], conditions=[LegalPose(O, P), LegalGrasp(O, G)],
        #           effects=[Kin(O, P, G, BQ, MQ)], generator=sample_motion),
        EasyListGenStream(inputs=[O, P, G],
                          outputs=[BQ, MQ],
                          conditions=[LegalPose(O, P),
                                      LegalGrasp(O, G)],
                          effects=[Kin(O, P, G, BQ, MQ),
                                   LegalConf(BQ, MQ)],
                          generator=sample_motion),
        # TODO - this doesn't work for constants

        # TODO - make a condition that MQ, MQ2 both work for BQ. Otherwise, it will still create a ton of streams
        #EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
        #           effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=lambda *args: [None], order=1),
        EasyGenStream(inputs=[MQ, MQ2, BQ],
                      outputs=[MT],
                      conditions=[TransitManip(MQ),
                                  LegalConf(BQ, MQ2)],
                      effects=[ManipMotion(MQ, MQ2, BQ, MT)],
                      generator=lambda *args: [None],
                      order=1),
        EasyGenStream(inputs=[MQ, MQ2, BQ],
                      outputs=[MT],
                      conditions=[LegalConf(BQ, MQ),
                                  TransitManip(MQ2)],
                      effects=[ManipMotion(MQ, MQ2, BQ, MT)],
                      generator=lambda *args: [None],
                      order=1),
        #EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[LegalConf(BQ, MQ), LegalConf(BQ, MQ2)],
        #           effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=lambda *args: [None], order=1),
        EasyGenStream(inputs=[BQ, BQ2],
                      outputs=[BT],
                      conditions=[],
                      effects=[BaseMotion(BQ, BQ2, BT)],
                      generator=lambda *args: [None],
                      order=1),
        #effects=[BaseMotion(BQ, BQ2, BT)], generator=lambda *args: [None], order=2), # NOTE - causes a bug!

        #EasyTestStream(inputs=[O, P, T], conditions=[LegalPose(O, P)], effects=[CFree(O, P, T)],
        #            test=collision_free, eager=EAGER_TESTS),
    ]

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

    #print transit_conf.value
    #print oracle.initial_config.value

    initial_base = get_base_conf(oracle.initial_config)
    initial_manip = get_arm_conf(oracle.initial_config)
    print initial_base.value
    print initial_manip.value

    constants = []

    initial_atoms = [
        BaseEq(initial_base),
        ManipEq(initial_manip),
        HandEmpty(),
        TransitManip(transit_conf),
        LegalConf(initial_base, initial_manip),
    ]
    for obj, pose in oracle.initial_poses.iteritems():
        initial_atoms += [PoseEq(obj, pose), LegalPose(obj, pose)]

    # TODO - serialize goals by object name
    goal_literals = []
    if problem.goal_holding is not None:
        if problem.goal_holding is False:
            goal_literals.append(HandEmpty())
        elif isinstance(problem.goal_holding, ObjGrasp):
            goal_literals.append(
                GraspEq(problem.goal_holding.object_name,
                        problem.goal_holding.grasp))
        elif problem.goal_holding in oracle.get_objects():
            goal_literals.append(Holding(problem.goal_holding))
        else:
            raise Exception()
    for obj, pose in problem.goal_poses.iteritems():
        goal_literals.append(PoseEq(obj, pose))
        initial_atoms.append(LegalPose(obj, pose))
    for obj, region in problem.goal_regions.iteritems():
        goal_literals.append(InRegion(obj, region))

    #goal_formula = And(*goal_literals)
    #goal_formula = AbsCondition(map(And, goal_literals)) # TODO - bug where goals must have And
    goal_formula = AbsCondition(
        goal_literals)  # TODO - bug where goals must have And

    return STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
                              cond_streams, constants)
コード例 #15
0
def amber_stripstream(use_window=True, step=False):
    problem = dantam2()
    window_3d = Window3D(title='World',
                         windowWidth=500,
                         noWindow=(not use_window),
                         viewport=problem.workspace.flatten('F'))

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

        # Collisions
        EasyTestStream(inputs=[P, P2],
                       conditions=[],
                       effects=[CFreePose(P, P2)],
                       test=cfree_pose,
                       eager=True),
        EasyTestStream(inputs=[T, P],
                       conditions=[],
                       effects=[CFreeTraj(T, P)],
                       test=cfree_traj),
    ]

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

    constants = map(GRASP, TOP_GRASPS[:MAX_GRASPS]) + map(
        POSE, problem.known_poses)
    initial_atoms = [
        ConfEq(problem.initial_conf),
        HandEmpty(),
    ] + [
        PoseEq(get_name(body), pose) for body, pose in problem.initial_poses
        if get_name(body) in problem.movable_names
    ]
    goal_literals = [
        PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()
    ]
    if problem.goal_conf is not None:
        goal_literals.append(ConfEq(problem.goal_conf))
    stream_problem = STRIPStreamProblem(initial_atoms, And(*goal_literals),
                                        actions + axioms, cond_streams,
                                        constants)

    search_fn = get_fast_downward('eager', max_time=10, verbose=False)
    plan, _ = incremental_planner(stream_problem,
                                  search=search_fn,
                                  frequency=1,
                                  waves=True,
                                  debug=False)

    print SEPARATOR

    plan = convert_plan(plan)
    if plan is not None:
        print 'Success'
        for i, (action, args) in enumerate(plan):
            print i + 1, action
    else:
        print 'Failure'

    if is_window_active(window_3d) and plan is not None:
        robot_conf = problem.initial_conf
        placements = {
            get_name(body): body.applyTrans(pose)
            for body, pose in problem.initial_poses
        }
        attached = {'left': None, 'right': None}

        def _execute_traj(path):
            for j, conf in enumerate(path):
                draw_window_3d(window_3d, conf, placements.values(), attached)
                if step:
                    raw_input('%s/%s) Step?' % (j, len(path)))
                else:
                    window_3d.update()
                    sleep(0.01)

        draw_window_3d(window_3d, robot_conf, placements.values())
        raw_input('Start?')
        for i, (action, args) in enumerate(plan):
            if action.name == 'move':
                _, _, traj = args
                _execute_traj(traj.path)
            elif action.name == 'move_holding':
                _, _, traj, _, _ = args
                _execute_traj(traj.path)
            elif action.name == 'pick':
                obj, _, grasp, _, traj = args
                _execute_traj(traj.path[::-1])
                attached[ARM] = name_to_body[obj].applyTrans(
                    get_wrist_frame(grasp, robot, ARM).inverse())
                del placements[obj]
                _execute_traj(traj.path)
            elif action.name == 'place':
                obj, pose, _, _, traj = args
                _execute_traj(traj.path[::-1])
                placements[obj] = name_to_body[obj].applyTrans(pose)
                attached[ARM] = None
                _execute_traj(traj.path)
            else:
                raise ValueError(action.name)

    raw_input('Finish?')
コード例 #16
0
def create_problem(initRobotPos = (0.5, 0.5),
                   initRobotVar = 0.01,
                   maxMoveDist = 5.0,
                   beaconPos = (1, 1),
                   homePos = (0, 0),
                   goalPosEps = 0.1,
                   goalVar = 0.1,
                   odoErrorRate = 0.1,
                   obsVarPerDistFromSensor = 10.0,
                   minObsVar = 0.001,
                   domainSize = 20,
                   verboseFns = True):
  """
  :return: a :class:`.STRIPStreamProblem`
  """
  # Data types
  POS = Type()   # 2D position
  VAR = Type()   # 2D variance
  DIST = Type()  # positive scalar

  # Fluent predicates
  RobotPos = Pred(POS)
  RobotVar = Pred(VAR)

  # Derived predicates
  KnowYouAreHome = Pred()

  # Static predicates
  DistBetween = Pred(POS, POS, DIST) # Distance between two positions (function)
  LessThanV = Pred(VAR, VAR) # Less than, on distances
  LessThanD = Pred(DIST, DIST) # Less than, on distances
  OdometryVar = Pred(VAR, VAR, DIST) # How does odometry variance increase?
  SensorVar = Pred(VAR, VAR, DIST) # How does an observation decrease variance?
  LegalPos = Pred(POS) # Any legal robot pos

  # Free parameters
  RPOS1, RPOS2, RVAR1, RVAR2 = Param(POS), Param(POS), Param(VAR), Param(VAR)
  DIST1, DIST2 = Param(DIST), Param(DIST)

  def odoVarFun(rv, d):
    odoVar = (d * odoErrorRate)**2
    result = rv + odoVar
    if verboseFns: print 'ovf:', rv, d, result
    return [result]

  def sensorVarFun(rv, d):
    obsVar = max(d / obsVarPerDistFromSensor, minObsVar)
    result = 1.0 / ((1.0 / rv) + (1.0 / obsVar))
    if verboseFns: print 'svf:', rv, d, result
    return [result]

  def randPos():
    while True:
      result = (random.random() * domainSize, random.random() * domainSize)
      print 'rp:', result
      yield [result]

  def legalTest(rp):
    (x, y) = rp
    result = (0 <= x <= domainSize) and (0 <= y <= domainSize)
    if not result: print 'not legal:', rp
    return result

  actions = [
    Action(name='Move',
           parameters=[RPOS1, RPOS2, RVAR1, RVAR2, DIST1],
           condition = And(RobotPos(RPOS1),
                           RobotVar(RVAR1),
                           LegalPos(RPOS2), # Generate an intermediate pos
                           DistBetween(RPOS1, RPOS2, DIST1),
                           LessThanD(DIST1, maxMoveDist),
                           OdometryVar(RVAR1, RVAR2, DIST1)),
           effect = And(RobotPos(RPOS2),
                        RobotVar(RVAR2),
                        Not(RobotPos(RPOS1)),
                        Not(RobotVar(RVAR1)))),

    # Action(name='Look',
    #        parameters=[RPOS1, RVAR1, RVAR2, DIST1],
    #        condition = And(RobotPos(RPOS1),
    #                        RobotVar(RVAR1),
    #                        DistBetween(RPOS1, beaconPos, DIST1),
    #                        SensorVar(RVAR1, RVAR2, DIST1)),
    #        effect = And(RobotVar(RVAR2),
    #                     Not(RobotVar(RVAR1))))

  ]

  axioms = [
    Axiom(effect = KnowYouAreHome(),
          condition = Exists([RPOS1, RVAR1, DIST1], 
                             And(RobotPos(RPOS1),
                                 RobotVar(RVAR1),
                                 DistBetween(RPOS1, homePos, DIST1),
                                 LessThanD(DIST1, goalPosEps),
                                 LessThanV(RVAR1, goalVar))))
  ]

  # Conditional stream declarations
  cond_streams = [
    TestStream(inputs = [RPOS1],
               conditions = [],
               effects = [LegalPos(RPOS1)],
               test = legalTest,
               eager = True),

    GeneratorStream(inputs = [],
                    outputs = [RPOS1],
                    conditions = [],
                    effects = [LegalPos(RPOS1)],
                    generator = randPos),

    GeneratorStream(inputs = [RPOS1, RPOS2],
                    outputs = [DIST1],
                    conditions = [],
                    effects = [DistBetween(RPOS1, RPOS2, DIST1)],
                    generator = lambda rp1, rp2: [distance(rp1, rp2)]),

    GeneratorStream(inputs = [RVAR1, DIST1],
                    outputs = [RVAR2],
                    conditions = [],
                    effects = [OdometryVar(RVAR1, RVAR2, DIST1)],
                    generator = odoVarFun),

    # GeneratorStream(inputs = [RVAR1, DIST1],
    #                 outputs = [RVAR2],
    #                 conditions = [],
    #                 effects = [SensorVar(RVAR1, RVAR2, DIST1)],
    #                 generator = sensorVarFun),

    TestStream(inputs = [DIST1, DIST2],
               conditions = [],
               effects = [LessThanD(DIST1, DIST2)],
               test = lt,
               eager = True),

    TestStream(inputs = [RVAR1, RVAR2],
               conditions = [],
               effects = [LessThanV(RVAR1, RVAR2)],
               test = lt, eager = True)

  ]

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

  constants = [
  ]

  initial_atoms = [
    RobotPos(initRobotPos),
    RobotVar(initRobotVar),
    LegalPos(homePos),
    LegalPos((.1, .1)),
    LegalPos((3.0, .1)),
    LegalPos((6.0, .1))
  ]

  goal_literals = [
    KnowYouAreHome()
    ]

  problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
                                 cond_streams, constants)

  return problem
コード例 #17
0
def solve_tamp(env, use_focused):
    use_viewer = env.GetViewer() is not None
    problem = PROBLEM(env)
    # TODO: most of my examples have literally had straight-line motions plans

    robot, manipulator, base_manip, ir_model = initialize_openrave(
        env, ARM, min_delta=MIN_DELTA)
    set_manipulator_conf(ir_model.manip, CARRY_CONFIG)
    bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names}
    surfaces = {surface.name: surface for surface in problem.surfaces}
    open_gripper(manipulator)
    initial_q = Conf(robot.GetConfigurationValues())
    initial_poses = {
        name: Pose(name, get_pose(body))
        for name, body in bodies.iteritems()
    }
    # TODO: just keep track of the movable degrees of freedom
    # GetActiveDOFIndices, GetActiveJointIndices, GetActiveDOFValues
    saver = EnvironmentStateSaver(env)

    #cfree_pose = cfree_pose_fn(env, bodies)
    #cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2, all_bodies)

    #base_generator = base_generator_fn(ir_model)

    #base_values = base_values_from_full_config(initial_q.value)
    #goal_values = full_config_from_base_values(base_values + np.array([1, 0, 0]), initial_q.value)
    #goal_conf = Conf(goal_values)
    #return

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

    # TODO - should objects contain their geometry

    cond_streams = [
        EasyListGenStream(inputs=[O, P, G],
                          outputs=[Q, T],
                          conditions=[IsPose(O, P),
                                      IsGrasp(O, G)],
                          effects=[GraspMotion(O, P, G, Q, T)],
                          generator=sample_grasp_traj_fn(
                              base_manip, ir_model, bodies, CARRY_CONFIG)),
        EasyGenStream(inputs=[Q, Q2],
                      outputs=[T],
                      conditions=[],
                      effects=[FreeMotion(Q, Q2, T)],
                      generator=sample_free_base_motion_fn(base_manip, bodies),
                      order=1,
                      max_level=0),
        EasyTestStream(
            inputs=[O, P, O2, P2],
            conditions=[IsPose(O, P), IsPose(O2, P2)],
            effects=[CFreePose(P, P2)],
            test=lambda *args: True,  #cfree_pose,
            eager=True),
        EasyTestStream(inputs=[T, P],
                       conditions=[],
                       effects=[CFreeTraj(T, P)],
                       test=lambda *args: True),
        #test=cfree_traj),
        EasyListGenStream(inputs=[O],
                          outputs=[G],
                          conditions=[],
                          effects=[IsGrasp(O, G)],
                          generator=grasp_generator_fn(bodies, TOP_GRASPS,
                                                       SIDE_GRASPS,
                                                       MAX_GRASPS)),
        EasyListGenStream(inputs=[O, S],
                          outputs=[P],
                          conditions=[],
                          effects=[IsPose(O, P), Stable(P, S)],
                          generator=pose_generator_fn(bodies, surfaces)),

        #EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)],
        #            effects=[], generator=base_generator),
    ]

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

    constants = []
    initial_atoms = [ConfEq(initial_q), HandEmpty()]
    for name in initial_poses:
        initial_atoms += body_initial_atoms(name, initial_poses, bodies,
                                            surfaces)

    goal_formula = And(
        ConfEq(initial_q), *[
            OnSurface(obj, surface)
            for obj, surface in problem.goal_surfaces.iteritems()
        ])
    #goal_formula = ConfEq(goal_conf)
    #obj, _ = problem.goal_surfaces.items()[0]
    #goal_formula = And(Holding(obj))
    #goal_formula = Holding(obj) # TODO: this cause a bug

    stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
                                        actions + axioms, cond_streams,
                                        constants)

    print stream_problem
    handles = draw_affine_limits(robot)
    if use_viewer:
        for surface in problem.surfaces:
            surface.draw(env)
        raw_input('Start?')

    max_time = INF
    search_fn = get_fast_downward('eager', max_time=10, verbose=False)
    if use_focused:
        solve = lambda: simple_focused(stream_problem,
                                       search=search_fn,
                                       max_level=INF,
                                       shared=False,
                                       debug=False,
                                       verbose=False,
                                       max_time=max_time)
    else:
        solve = lambda: incremental_planner(stream_problem,
                                            search=search_fn,
                                            frequency=10,
                                            waves=False,
                                            debug=False,
                                            max_time=max_time)
    with env:
        plan, universe = solve()

    print SEPARATOR

    plan = convert_plan(plan)
    if plan is not None:
        print 'Success'
        for i, (action, args) in enumerate(plan):
            print i + 1, action, args
    else:
        print 'Failure'

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

    if plan is not None:
        commands = process_plan(robot, bodies, plan)
        if use_viewer:
            print SEPARATOR
            saver.Restore()
            visualize_solution(commands, step=False)
    raw_input('Finish?')
コード例 #18
0
def compile_problem(oracle):
    problem = oracle.problem

    block_poses = [(C(obj, BLOCK), Pose(obj, oracle.initial_poses[obj]))
                   for obj in oracle.get_objects()]

    #region = C('region%s'%1, REGION)
    initial_atoms = [
        HandEmpty(),  # TODO - toggle
    ] + [AtPose(block, pose) for block, pose in block_poses
         ] + [IsPose(block, pose) for block, pose in block_poses]

    goal_literals = []
    if problem.goal_holding is not None:
        if problem.goal_holding is False:
            goal_literals.append(HandEmpty())
        elif isinstance(problem.goal_holding, Holding):
            goal_literals.append(
                HasGrasp(C(problem.goal_holding.object_name, BLOCK),
                         C(problem.goal_holding.grasp, GRASP)))
        elif problem.goal_holding in oracle.get_objects():
            goal_literals.append(Holding(C(problem.goal_holding, BLOCK)))
        else:
            raise Exception()
    for obj in problem.goal_poses:
        if problem.goal_poses[obj] == 'initial':
            problem.goal_poses[obj] = oracle.initial_poses[obj]
        elif problem.goal_poses[obj] in oracle.initial_poses:
            problem.goal_poses[obj] = oracle.initial_poses[
                problem.goal_poses[obj]]
        goal_literals.append(
            AtPose(C(obj, BLOCK), C(obj, problem.goal_poses[obj])))
    for obj, region in problem.goal_regions.iteritems():
        #for obj, region in problem.goal_regions.items()[:1]:
        goal_literals.append(InRegion(C(obj, BLOCK), C(region, REGION)))

    # NOTE - need to add the IsPose to everything a priori
    """
  block = C(oracle.get_objects()[0], BLOCK)
  goal_literals = [
    Holding(block),
    #Not(HandEmpty(robot)), # NOTE - negative goal
    #Not(AtPose(obj, Pose(0))),
    #AtPose(block, Pose(20)),
    #AtPose(obj, Pose(4)),
    #AtConfig(Config(5)),
    #AtConfig(robot, Config(4)),
    #InRegion(block, region),
  ]
  """

    actions = [
        Pick(oracle),
        Place(oracle),
    ]

    axioms = [
        #HoldingAxiom(),
        InRegionAxiom(),
        SafeAxiom(),
    ]

    cond_streams = [
        PoseStream(oracle),
        GraspStream(oracle),
        ContainedStream(oracle),
        #ContainedTest(oracle),
        IKStream(oracle),
        CollisionFreeTest(oracle),
    ]

    objects = []

    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
                              cond_streams, objects)
コード例 #19
0
def create_problem(goal, obstacles=(), distance=.25, digits=3):
    """
  Creates a Probabilistic Roadmap (PRM) motion planning problem.

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

    # Data types
    POINT = Type()
    REGION = Type()

    # Fluent predicates
    AtPoint = Pred(POINT)

    # Derived predicates
    InRegion = Pred(REGION)

    # Stream predicates
    AreNearby = Pred(POINT, POINT)
    IsEdge = Pred(POINT, POINT)
    Contained = Pred(POINT, REGION)

    # Functions
    Distance = Func(POINT, POINT)

    # Free parameters
    P1, P2 = Param(POINT), Param(POINT)
    R = Param(REGION)

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

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

    actions = [
        #STRIPSAction(name='move', parameters=[P1, P2],
        #  conditions=[AtPoint(P1), IsEdge(P1, P2)],
        #  effects=[AtPoint(P2), Not(AtPoint(P1))], cost=1), # Fixed cost

        #STRIPSAction(name='move', parameters=[P1, P2],
        #  conditions=[AtPoint(P1), IsEdge(P1, P2)],
        #  effects=[AtPoint(P2), Not(AtPoint(P1)), Cost(Distance(P1, P2))]), # Cost depends on parameters
        Action(name='move',
               parameters=[P1, P2],
               condition=And(AtPoint(P1), IsEdge(P1, P2)),
               effect=And(AtPoint(P2), Not(AtPoint(P1))),
               costs=[1, Distance(P1, P2)]),
    ]

    axioms = [
        #Axiom(effect=GoalReached(), condition=Exists([P1], And(AtPos(P1), Contained(P1)))),
        STRIPSAxiom(conditions=[AtPoint(P1), Contained(P1, R)],
                    effects=[InRegion(R)])
    ]

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

    # Conditional stream declarations
    cond_streams = [
        GeneratorStream(
            inputs=[],
            outputs=[P1],
            conditions=[],
            effects=[],
            generator=lambda: ((sample(digits), ) for _ in inf_sequence())
        ),  # NOTE - version that only generators collision-free points
        GeneratorStream(inputs=[R],
                        outputs=[P1],
                        conditions=[],
                        effects=[Contained(P1, R)],
                        generator=lambda r:
                        ((sample_box(r), ) for _ in inf_sequence())),

        #TestStream(inputs=[P1, P2], conditions=[], effects=[AreNearby(P1, P2), AreNearby(P2, P1)],
        #           test=lambda p1, p2: get_distance(p1, p2) <= distance, eager=True),

        #TestStream(inputs=[P1, P2], conditions=[AreNearby(P1, P2)], effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
        #           test=lambda p1, p2: is_collision_free((p1, p2), obstacles), eager=True),
        TestStream(
            inputs=[P1, P2],
            conditions=[],
            effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
            #test=lambda p1, p2: is_collision_free((p1, p2), obstacles), eager=True),
            test=lambda p1, p2:
            (get_distance(p1, p2) <= distance) and is_collision_free(
                (p1, p2), obstacles),
            eager=True),

        #TestStream(inputs=[P1, P2], conditions=[], effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
        #           test=lambda p1, p2: get_distance(p1, p2) <= distance and is_collision_free((p1, p2), obstacles), eager=True),

        #TestStream(inputs=[P1, R], conditions=[], effects=[Contained(P1, R)],
        #           test=lambda p, r: contains(p, r), eager=True),
        CostStream(inputs=[P1, P2],
                   conditions=[],
                   effects=[Distance(P1, P2),
                            Distance(P2, P1)],
                   function=get_distance,
                   scale=100,
                   eager=True),
    ]

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

    constants = []

    initial_atoms = [
        AtPoint((0, 0)),
    ]

    goal_literals = []
    if is_region(goal):
        goal_literals.append(InRegion(goal))
    else:
        goal_literals.append(AtPoint(goal))

    problem = STRIPStreamProblem(initial_atoms, goal_literals,
                                 actions + axioms, cond_streams, constants)

    return problem
コード例 #20
0
def create_problem():
  """
  Creates the 1D task and motion planning STRIPStream problem.

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

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

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

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

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

  VALUE = Type()

  # Fluent predicates
  AtConf = Pred(VALUE)
  AtPose = Pred(VALUE, VALUE)
  HandEmpty = Pred()
  Holding = Pred(VALUE)

  # Derived predicates
  Safe = Pred(VALUE, VALUE, VALUE)

  # Static predicates
  IsBlock = Pred(VALUE)
  IsPose = Pred(VALUE)
  IsConf = Pred(VALUE)
  LegalKin = Pred(VALUE, VALUE)
  CollisionFree = Pred(VALUE, VALUE, VALUE, VALUE)

  # Free parameters
  B1, B2 = Param(VALUE), Param(VALUE)
  P1, P2 = Param(VALUE), Param(VALUE)
  Q1, Q2 = Param(VALUE), Param(VALUE)

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

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

  # NOTE - it would be easier to just update an in hand pose

  actions = [
    STRIPSAction(name='pick', parameters=[B1, P1, Q1],
      conditions=[IsBlock(B1), IsPose(P1), IsConf(Q1), LegalKin(P1, Q1),
                  AtPose(B1, P1), HandEmpty(), AtConf(Q1)],
      effects=[Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty())]),

    STRIPSAction(name='place', parameters=[B1, P1, Q1],
      conditions=[IsBlock(B1), IsPose(P1), IsConf(Q1), LegalKin(P1, Q1),
                  Holding(B1), AtConf(Q1)] + [Safe(b2, B1, P1) for b2 in blocks],
      effects=[AtPose(B1, P1), HandEmpty(), Not(Holding(B1))]),

    STRIPSAction(name='move', parameters=[Q1, Q2],
      conditions=[IsConf(Q1), IsConf(Q2), AtConf(Q1)],
      effects=[AtConf(Q2), Not(AtConf(Q1))]),
  ]

  axioms = [ # TODO - need to combine axioms
    #Axiom(effect=Safe(B2, B1, P1),
    #      condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), # Infers B2 is at a safe pose wrt B1 at P1
  ]

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

  # Conditional stream declarations
  cond_streams = [
    GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[IsPose(P1)],
                    generator=lambda: xrange(num_poses)), # Enumerating all the poses

    GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[IsPose(P1)], effects=[IsConf(Q1), LegalKin(P1, Q1)],
                    generator=lambda p: [p]), # Inverse kinematics

    TestStream(inputs=[B1, P1, B2, P2], conditions=[IsBlock(B1), IsPose(P1), IsBlock(B2), IsPose(P2)],
               effects=[CollisionFree(B1, P1, B2, P2)], test=lambda b1, p1, b2, p2: p1 != p2, eager=True), # Collision checking
  ]

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

  constants = []

  initial_atoms = [
    IsConf(initial_config),
    AtConf(initial_config),
    HandEmpty()
  ]
  for block, pose in initial_poses.iteritems():
    initial_atoms += [
      IsBlock(block),
      IsPose(pose),
      AtPose(block, pose),
    ]
  goal_literals = []
  for block, pose in goal_poses.iteritems():
    initial_atoms += [
      IsBlock(block),
      IsPose(pose),
    ]
    goal_literals.append(AtPose(block, pose))

  problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)

  return problem
コード例 #21
0
def solve_incrementally():

    O = Param(OBJECT)
    O1, O2 = Param(OBJECT), Param(OBJECT)
    L = Param(LOCATION)
    L_s, L_g = Param(LOCATION), Param(LOCATION)  # generic location
    Stove_l_s, Stove_l_g = Param(STOVE_L_S), Param(
        STOVE_L_G)  # locations for stove and sink
    Sink_l_s, Sink_l_g = Param(SINK_L_S), Param(SINK_L_G)

    actions = [
        Action(name='wash',
               parameters=[O],
               condition=And(InSink(O)),
               effect=And(Clean(O))),
        Action(name='cook',
               parameters=[O],
               condition=And(InStove(O), Clean(O)),
               effect=And(Cooked(O))),
        Action(name='pickplace',
               parameters=[O, L_s, L_g],
               condition=And(EmptySweptVolume(O, L_s, L_g), AtPose(O, L_s)),
               effect=And(AtPose(O, L_g),
                          Not(AtPose(O, L_s))))  # You should delete!
    ]

    axioms = [
     # For all objects in the world, either object is O1 or if not, then it is not in the region
     Axiom(effect=EmptySweptVolume(O,L_s,L_g),condition=ForAll([O2],\
                                                    Or(Equal(O,O2),\
                                                        Exists([L],(And(AtPose(O2,L),OutsideRegion(O,O2,L,L_s,L_g))))))),
     # Object is in the stove if it is at pose L for which Ls and Lg define stove
     Axiom(effect=InStove(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsStove(L_s,L_g)))),
     Axiom(effect=InSink(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsSink(L_s,L_g)))),
    ]

    cond_streams = [
      EasyGenStream(inputs=[O,L_s,L_g], outputs=[L], conditions=[IsSmaller(L_s,L_g)], effects=[Contained(O,L,L_s,L_g)],\
                    generator=lambda b, ls, lg: (sample_region_pose(b, ls, lg ) for _ in irange(0, INF))),
      EasyTestStream(inputs=[L_s,L_g],conditions=[],effects=[IsSmaller(L_s,L_g)],test=is_smaller,eager=EAGER_TESTS),

      # Generate static predicates that object is contained in sink for which Ls and Lg define the sink. If L was not continuous value,
      # then we would define this in the intial condition and would not be changed by any of the actions (hence static predicate)
      EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsSink(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
      EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsStove(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),

      # OutsideRegion tests if O2 is is outside of the region (Ls,Lg)
      EasyTestStream(inputs=[O,O2,L,L_s,L_g],conditions=[],effects=[OutsideRegion(O,O2,L,L_s,L_g)],test=not_in_region,eager=EAGER_TESTS),
    ]

    ####################
    tamp_problem = sample_one_d_kitchen_problem()

    # instantiate the environment region?
    constants = [
        STOVE_L_S(tamp_problem.stove_region_s),
        STOVE_L_G(tamp_problem.stove_region_g),
        SINK_L_S(tamp_problem.sink_region_s),
        SINK_L_G(tamp_problem.sink_region_g),
    ]

    # define initial state using initial poses of objects
    initial_atoms = [
        AtPose(block, pose)
        for block, pose in tamp_problem.initial_poses.iteritems()
    ] + [IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)] + [
        IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
    ]  # initial_atoms = static predicates, but on steroid

    goal_literals = []
    subgoal_list = [ InSink(tamp_problem.target_obj), \
                     InStove(tamp_problem.target_obj), Cooked(tamp_problem.target_obj) ]
    for i in range(len(subgoal_list)):
        goal_literals = [subgoal_list[0]]

        stream_problem = STRIPStreamProblem(initial_atoms, goal_literals, \
                                              actions+axioms, cond_streams, constants)
        search = get_fast_downward('eager')
        plan, universe = incremental_planner(stream_problem, search=search,\
                                             frequency=1, verbose=True, max_time=200)
        plan = convert_plan(plan)
        # move the object to the new locations; todo: add new predicates
        if len(plan) > 0:  # if no action needed, keep last initial atoms
            initial_atoms = []
        for action in plan:
            action_name = action[0].name
            action_args = action[1]
            if action_name == 'pickplace':
                O = action_args[0].name
                pick_l = action_args[1]
                place_l = action_args[2]
                block = [b for b in tamp_problem.blocks if b.name == O][0]
                initial_atoms += [AtPose(block, place_l)]
        if len(initial_atoms) == 1:
            block = [b for b in tamp_problem.blocks if b.name != O][0]
            initial_atoms += [AtPose(block, tamp_problem.initial_poses[block])]
        initial_atoms += [
            IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)
        ]
        initial_atoms += [
            IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
        ]
コード例 #22
0
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

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

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

  # 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],
      condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)),
      effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),

    Action(name='place', parameters=[B1, P1, Q1],
      condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
        ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))), # TODO - convert to finite blocks case?
      effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),

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

  axioms = [
    Axiom(effect=Safe(B2, B1, P1),
          condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), # Infers B2 is at a safe pose wrt B1 at P1
  ]

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

  cond_streams = []
  constants = []

  initial_atoms = [
    AtConf(initial_config),
    HandEmpty()
  ] + [
    AtPose(block, pose) for block, pose in initial_poses.iteritems()
  ] + [
    LegalKin(i, i) for i in range(num_poses)
  ] + [
    CollisionFree(b1, p1, b2, p2) for b1, p1, b2, p2 in product(blocks, range(num_poses), blocks, range(num_poses)) if p1 != p2 and b1 != b2
  ]

  goal_literals = [AtPose(block, pose) for block, pose in goal_poses.iteritems()]

  problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)

  return problem
コード例 #23
0
def compile_problem(estimator, task):
    # Data types
    CONF = Type()
    SURFACE = Type()  # Difference between fixed and movable objects
    ITEM = Type()
    POSE = Type()
    CLASS = Type()

    # Fluent predicates
    AtConf = Pred(CONF)
    HandEmpty = Pred()
    Holding = Pred(ITEM)
    AtPose = Pred(ITEM, POSE)
    Supported = Pred(POSE, SURFACE)  # Fluent
    Localized = Pred(OBJECT)
    Measured = Pred(OBJECT)

    # Static predicates
    IsKin = Pred(POSE, CONF)
    IsClass = Pred(OBJECT, CLASS)
    IsVisible = Pred(SURFACE, CONF)
    IsSupported = Pred(POSE, SURFACE)  # Static

    # Functions
    ScanRoom = Func(SURFACE)
    #ScanTable = Func(SURFACE, TYPE)
    ScanTable = Func(
        SURFACE, ITEM)  # TODO: could include more specific vantage point costs
    Distance = Func(CONF, CONF)

    # Derived
    On = Pred(ITEM, SURFACE)
    ComputableP = Pred(POSE)
    ComputableQ = Pred(CONF)

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

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

    # TODO: could just do easier version of this that doesn't require localized to start

    actions = [
        Action(
            name='pick',
            parameters=[B1, P1, Q1],  # TODO: Visibility constraint
            condition=And(Localized(B1), AtPose(B1, P1), HandEmpty(),
                          AtConf(Q1), IsKin(P1, Q1)),
            effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
        Action(name='place',
               parameters=[B1, P1, Q1],
               condition=And(Holding(B1), AtConf(Q1), IsKin(P1, Q1)),
               effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
        Action(name='move',
               parameters=[Q1, Q2],
               condition=And(AtConf(Q1), ComputableQ(Q2)),
               effect=And(AtConf(Q2), Not(AtConf(Q1)), Cost(Distance(Q1,
                                                                     Q2)))),
        Action(name='scan_room',
               parameters=[S1],
               condition=Not(Localized(S1)),
               effect=And(Localized(S1), Cost(ScanRoom(S1)))),
        # TODO: need to set later poses to be usable or not to constrain order
        Action(name='scan_table',
               parameters=[S1, B1, P1, Q1],
               condition=And(Localized(S1), AtConf(Q1), IsVisible(S1, Q1),
                             Not(Localized(B1))),
               effect=And(Localized(B1), Measured(P1), Supported(P1, S1),
                          Cost(ScanTable(S1, B1)))),
    ]

    axioms = [
        # TODO: axiom for on? Might need a stream that generates initial fluents for On
        # TODO: axiom that says that all fake values depending on a certain one now are usable
        # TODO: could use stream predicates as fluents (as long as it doesn't break anything...)
        Axiom(effect=On(B1, S1),
              condition=Exists([P1],
                               And(AtPose(B1, P1),
                                   Or(IsSupported(P1, S1), Supported(P1,
                                                                     S1))))),

        # TODO: compile automatically
        Axiom(effect=ComputableQ(Q1),
              condition=Or(Measured(Q1),
                           Exists([P1], And(IsKin(P1, Q1), ComputableP(P1))),
                           Exists([S1], And(IsVisible(S1, Q1),
                                            Localized(S1))))),
        Axiom(effect=ComputableP(P1),
              condition=Or(
                  Measured(P1),
                  Exists([S1], And(IsSupported(P1, S1), Localized(S1))))),
    ]

    #####

    surface_types = estimator.surface_octomaps.keys()
    item_types = estimator.object_octomaps.keys()
    names_from_type = defaultdict(list)
    known_poses = {}
    holding = None

    def add_type(cl):
        name = '{}{}'.format(cl, len(names_from_type[cl]))
        names_from_type[cl].append(name)
        return name

    # TODO: this is all very similar to the generic open world stuff
    if estimator.holding is not None:
        holding = add_type(estimator.holding)
    for cl, octomap in estimator.surface_octomaps.items(
    ):  # TODO: generic surface object
        for pose in octomap.get_occupied():
            known_poses[add_type(cl)] = pose
    for cl, octomap in estimator.object_octomaps.items():
        for pose in octomap.get_occupied():
            known_poses[add_type(cl)] = pose
    print dict(names_from_type), known_poses

    # Human tells you to move block -> at least one block
    # At least one block -> at least one surface
    # TODO: generate fake properties about these fake values?
    goal_objects, goal_surfaces = entities_from_task(task)
    for cl in surface_types:
        add_type(cl)
    #for cl in goal_surfaces:
    #  for i in xrange(len(names_from_type[cl]), goal_surfaces[cl]):
    #    add_type(cl)
    #for cl in item_types:
    for cl in goal_objects:
        for i in xrange(len(names_from_type[cl]), goal_objects[cl]):
            add_type(cl)

    #####

    initial_atoms = [
        AtConf(estimator.robot_conf),
        Measured(CONF(estimator.robot_conf))
    ]
    if holding is None:
        initial_atoms.append(HandEmpty())
    else:
        initial_atoms.append(Holding(holding))

    class_from_name = {
        name: ty
        for ty in names_from_type for name in names_from_type[ty]
    }
    for name, ty in class_from_name.iteritems():
        ENTITY = SURFACE if ty in surface_types else ITEM
        initial_atoms.append(IsClass(ENTITY(name), ty))
        if name in known_poses:
            initial_atoms.append(Localized(ENTITY(name)))
            if ENTITY == ITEM:
                pose = known_poses[name]
                initial_atoms += [AtPose(name, pose), Measured(POSE(pose))]
        else:
            if ENTITY == ITEM:
                pose = 'p_init_{}'.format(
                    name
                )  # The object should always be at this pose (we just can't do anything about it yet)
                initial_atoms += [AtPose(name, pose)]

    goal_literals = []
    if task.holding is None:
        goal_literals.append(HandEmpty())
    elif task.holding is not False:
        goal_literals.append(
            Exists([B1], And(Holding(B1), IsClass(B1, task.holding))))
    for obj, surface in task.object_surfaces:
        goal_literals.append(
            Exists([B1, S1],
                   And(On(B1, S1), IsClass(B1, obj), IsClass(S1, surface))))
    goal_formula = And(*goal_literals)

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

    TOLERANCE = 0.1

    def is_visible(table, conf):
        x, y = conf
        pose = known_poses[table]
        #return (pose == x) and (y == 2)
        #return (pose == x) and (y == 2)
        return (pose == x) and (abs(y - 2) < TOLERANCE)

    def is_kinematic(pose, conf):
        x, y = conf
        #return (pose == x) and (y == 1)
        return (pose == x) and (abs(y - 1) < TOLERANCE)

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

    def sample_visible(table):  # TODO: could generically do this with poses
        if table in known_poses:
            y = 2
            #y += round(uniform(-TOLERANCE, TOLERANCE), 3)
            conf = (known_poses[table], y)
            assert is_visible(table, conf)
        else:
            conf = 'q_vis_{}'.format(table)
        yield (conf, )

    def inverse_kinematics(pose):  # TODO: list stream that uses ending info
        # TODO: only do if localized as well?
        # TODO: is it helpful to have this even if the raw value is kind of wrong (to steer the search)
        if type(pose) != str:
            y = 1
            #y += round(uniform(-TOLERANCE, TOLERANCE), 3)
            conf = (pose, y)
            assert is_kinematic(pose, conf)
        else:
            conf = 'q_ik_{}'.format(pose)
        yield (conf, )

    def sample_table(table):
        if table in known_poses:
            pose = known_poses[table]
        else:
            pose = 'p_{}'.format(table)
        yield (pose, )

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

    MAX_DISTANCE = 10

    # TODO: maybe I don't need to worry about normalizing. I can just pretend non-parametric again for planning
    def scan_surface_cost(
            surface, obj):  # TODO: what about multiple scans of the belief?
        fail_cost = 100
        surface_cl = class_from_name[surface]
        obj_cl = class_from_name[obj]
        prob = 1.0
        if obj_cl in estimator.object_prior:
            prob *= estimator.object_prior[obj_cl].get(surface_cl, 0)
        if surface in known_poses:
            prob *= estimator.object_octomaps[obj_cl].get_prob(
                known_poses[surface])
        else:
            prob *= 0.1  # Low chance if you don't even know the table exists
            # TODO: could even include the probability the table exists
        #return expected_cost(1, fail_cost, prob)
        return mdp_cost(1, fail_cost, prob)

    def scan_room_cost(surface):
        # TODO: try to prove some sort of bound on the cost to recover will suffice?
        fail_cost = 100
        cl = class_from_name[surface]
        occupied_poses = {
            known_poses[n]
            for n in names_from_type[cl] if n in known_poses
        }
        p_failure = 1.0
        for pose in estimator.poses:
            if pose not in occupied_poses:
                p_failure *= (1 -
                              estimator.surface_octomaps[cl].get_prob(pose))
        return 1 * (1 - p_failure) + fail_cost * p_failure

    def distance_cost(q1, q2):
        if str in (type(q1), type(q2)):
            return MAX_DISTANCE  # TODO: take the max possible pose distance
        # TODO: can use the info encoded within these to obtain better bounds
        return np.linalg.norm(np.array(q2) - np.array(q1))

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

    # TODO: could add measured as the output to these
    streams = [
        GeneratorStream(inputs=[P1],
                        outputs=[Q1],
                        conditions=[],
                        effects=[IsKin(P1, Q1)],
                        generator=inverse_kinematics),
        GeneratorStream(inputs=[S1],
                        outputs=[Q1],
                        conditions=[],
                        effects=[IsVisible(S1, Q1)],
                        generator=sample_visible),
        GeneratorStream(inputs=[S1],
                        outputs=[P1],
                        conditions=[],
                        effects=[IsSupported(P1, S1)],
                        generator=sample_table),
        CostStream(inputs=[S1, B1],
                   conditions=[],
                   effects=[ScanTable(S1, B1)],
                   function=scan_surface_cost),
        CostStream(inputs=[Q1, Q2],
                   conditions=[],
                   effects=[Distance(Q1, Q2),
                            Distance(Q2, Q1)],
                   function=distance_cost),
        CostStream(inputs=[S1],
                   conditions=[],
                   effects=[ScanRoom(S1)],
                   function=scan_room_cost),

        # TODO: make an is original precondition and only apply these to original values?
        # I suppose I could apply to all concrete things but that's likely not useful
        #TestStream(inputs=[S1, Q1], conditions=[IsOriginal(Q1)], effects=[IsVisible(S1, Q1)],
        #           test=is_visible, eager=True),
        #TestStream(inputs=[P1, Q1], conditions=[IsOriginal(Q1), IsOriginal(Q1)], effects=[IsKin(P1, Q1)],
        #           test=is_kinematic, eager=True),

        #GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[IsVisible(P1, Q1)],
        #                generator=sample_visible),
    ]

    problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
                                 streams, [])

    def command_from_action((action, args)):
        if action.name == 'scan_room':
            return simulate_scan, []
        if action.name in ('scan_table', 'look_block'):
            return simulate_look, []
        if action.name == 'move':
            q1, q2 = map(get_value, args)
            return simulate_move, [q2]
        if action.name == 'pick':
            o, p, q = map(get_value, args)
            return simulate_pick, [class_from_name[o]]
        if action.name == 'place':
            return simulate_place, []
        raise ValueError(action.name)

    return problem, command_from_action
コード例 #24
0
def compile_problem(tamp_problem):
    """
    Constructs a STRIPStream problem for the countable TMP problem.

    :param tamp_problem: a :class:`.TMPProblem`
    :return: a :class:`.STRIPStreamProblem`
    """

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

    actions = [
        Action(name='pick',
               parameters=[B1, P1, Q1],
               condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
                             LegalKin(P1, Q1)),
               effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
        Action(name='place',
               parameters=[B1, P1, Q1],
               condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
                             ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),
               effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
        Action(name='move',
               parameters=[Q1, Q2],
               condition=AtConf(Q1),
               effect=And(AtConf(Q2), Not(AtConf(Q1)))),
    ]

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

    cond_streams = [
        EasyGenStream(inputs=[],
                      outputs=[P1],
                      conditions=[],
                      effects=[],
                      generator=lambda: irange(0, NUM_POSES)),
        EasyGenStream(inputs=[P1],
                      outputs=[Q1],
                      conditions=[],
                      effects=[LegalKin(P1, Q1)],
                      generator=lambda p: iter([p])),
        EasyTestStream(inputs=[P1, P2],
                       conditions=[],
                       effects=[CollisionFree(P1, P2)],
                       test=lambda p1, p2: p1 != p2,
                       eager=EAGER_TESTS),
    ]

    constants = []

    initial_atoms = [
        AtConf(tamp_problem.initial_config),
    ] + [
        AtPose(block, pose)
        for block, pose in tamp_problem.initial_poses.iteritems()
    ]
    if tamp_problem.initial_holding is False:
        initial_atoms.append(HandEmpty())
    else:
        initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK))

    goal_literals = []
    if tamp_problem.goal_holding is False:
        goal_literals.append(HandEmpty())
    elif tamp_problem.goal_holding is not None:
        goal_literals.append(Holding(tamp_problem.goal_holding))
    for block, goal in tamp_problem.goal_poses.iteritems():
        goal_literals.append(AtPose(block, goal))

    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
                              cond_streams, constants)
コード例 #25
0
def create_problem(dt, GoalTest, ClearTest, CalcDiffSystem, InitialState):
    """
  Creates a generic non-holonomic motion planning problem
  :return: a :class:`.STRIPStreamProblem`
  """

    # Data types
    X, DX, U = Type(), Type(), Type()

    # Fluent predicates
    AtX = Pred(X)

    # Fluent predicates
    GoalReached = Pred()

    # Static predicates
    ComputeDX = Pred(X, U, DX)
    Dynamics = Pred(X, DX, X)
    AtGoal = Pred(X)
    Clear = Pred(X, X)

    # Free parameters
    X1, X2 = Param(X), Param(X)
    DX1 = Param(DX)
    U1 = Param(U)
    rename_easy(locals())  # Trick to make debugging easier

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

    actions = [
        Action(name='simulate',
               parameters=[X1, U1, X2, DX1],
               condition=And(AtX(X1), ComputeDX(X1, U1, DX1),
                             Dynamics(X1, DX1, X2), Clear(X1, X2)),
               effect=And(Not(AtX(X1)), AtX(X2))),
    ]

    axioms = [
        Axiom(effect=GoalReached(), condition=Exists([X1], AtGoal(X1))),
    ]

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

    # Conditional stream declarations
    def ComputeDXCalculator(X1F, DX1F):
        X2F = X1F[:]
        for i in range(len(X2F)):
            X2F[i] = X1F[i] + dt * DX1F[i]
        return X2F

    cond_streams = [
        FunctionStream(inputs=[X1, DX1],
                       outputs=[X2],
                       conditions=[],
                       effects=[Dynamics(X1, DX1, X2)],
                       function=ComputeDXCalculator),
        FunctionStream(inputs=[X1, U1],
                       outputs=[DX1],
                       conditions=[],
                       effects=[ComputeDX(X1, U1, DX1)],
                       function=CalcDiffSystem),
        TestStream(inputs=[X1, X2],
                   conditions=[],
                   effects=[Clear(X1, X2)],
                   test=ClearTest,
                   eager=True),
        TestStream(inputs=[X1],
                   conditions=[],
                   effects=[AtGoal(X1)],
                   test=GoalTest,
                   eager=True),
    ]

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

    constants = [
        # TODO - need to declare control inputs (or make a stream for them)
    ]

    initial_atoms = [
        AtX(InitialState),
    ]

    goal_literals = [GoalReached()]

    problem = STRIPStreamProblem(initial_atoms, goal_literals,
                                 actions + axioms, cond_streams, constants)

    return problem
コード例 #26
0
def compile_problem(oracle):
  problem = oracle.problem
  for obj in problem.goal_poses:
    if problem.goal_poses[obj] == 'initial':
      problem.goal_poses[obj] = oracle.initial_poses[obj]
    elif problem.goal_poses[obj] in oracle.initial_poses: # Goal names other object (TODO - compile with initial)
      problem.goal_poses[obj] = oracle.initial_poses[problem.goal_poses[obj]]

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

  def sample_poses(o, num_samples=1):
    while True:
      if AVOID_INITIAL:
        oracle.set_all_object_poses(oracle.initial_poses)
      else:
        oracle.set_all_object_poses({o: oracle.initial_poses[o]})
      yield list(islice(random_region_placements(oracle, o, oracle.get_counters(), region_weights=True), num_samples))

  def sample_grasps(o):
    yield get_grasps(oracle, o)

  def sample_region(o, r, num_samples=1):
    while True:
      oracle.set_all_object_poses({o: oracle.initial_poses[o]})
      yield list(islice(random_region_placements(oracle, o, [r], region_weights=True), num_samples))

  def sample_motion(o, p, g, max_calls=1, max_failures=50):
    oracle.set_all_object_poses({o: p}) # TODO - saver for the initial state as well?
    if oracle.approach_collision(o, p, g):
      return
    for i in range(max_calls):
      pap = PickAndPlace(oracle.get_geom_hash(o), p, g)
      if not pap.sample(oracle, o, max_failures=max_failures,
                        sample_vector=DO_ARM_MOTION, sample_arm=DO_ARM_MOTION, check_base=CHECK_BASE):
        break
      pap.obj = o
      yield [(pap.approach_config, pap)]

  def collision_free(o, p, t):
    if p is None or o == t.obj:
      return True
    holding = ObjGrasp(t.obj, t.grasp)
    #holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
    if not DO_ARM_MOTION:
      return not oracle.holding_collision(t.grasp_config, o, p, holding)
    return not oracle.traj_holding_collision(t.approach_config, t.trajs, o, p, holding)

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

  # Types
  CONF, TRAJ, REG = Type(), Type(), Type()
  BLOCK, POSE, GRASP = Type(), Type(), Type()

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

  # Static predicates
  IsPose = Pred(BLOCK, POSE)
  IsGrasp = Pred(BLOCK, GRASP)
  IsKin = Pred(BLOCK, POSE, GRASP, CONF, TRAJ)
  IsCollisionFree = Pred(BLOCK, POSE, TRAJ)
  IsContained = Pred(REG, BLOCK, POSE)

  # Derived predicates
  Safe = Pred(BLOCK, TRAJ)
  InRegion = Pred(BLOCK, REG)

  # Parameters
  O, P, G = Param(BLOCK), Param(POSE), Param(GRASP)
  Q, Q2, T = Param(CONF), Param(CONF), Param(TRAJ)
  OB, R = Param(BLOCK), Param(REG)

  actions = [
    Action(name='pick', parameters=[O, P, G, Q, T],
      condition=And(AtPose(O, P), HandEmpty(),
        IsKin(O, P, G, Q, T), AtConfig(Q),
        ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
      effect=And(Holding(O, G),
        Not(HandEmpty()), Not(AtPose(O, P)))),
    Action(name='place', parameters=[O, P, G, Q, T],
      condition=And(Holding(O, G),
        IsKin(O, P, G, Q, T), AtConfig(Q),
        ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
      effect=And(AtPose(O, P), HandEmpty(),
        Not(Holding(O, G)))),
    Action(name='move', parameters=[Q, Q2],
      condition=AtConfig(Q),
      effect=And(AtConfig(Q2),
        Not(AtConfig(Q))))]

  axioms = [
    Axiom(effect=InRegion(O, R), condition=Exists([P],
      And(AtPose(O, P), IsContained(R, O, P)))),
    Axiom(effect=Safe(O, T), condition=Exists([P],
      And(AtPose(O, P), IsCollisionFree(O, P, T))))]

  cond_streams = [
    GenStream(inputs=[O], outputs=[P],
      conditions=[],
      effects=[IsPose(O, P)],
      generator=sample_poses),
    GenStream(inputs=[O], outputs=[G],
      conditions=[],
      effects=[IsGrasp(O, G)],
      generator=sample_grasps),
    GenStream(inputs=[O, R], outputs=[P],
      conditions=[],
      effects=[IsPose(O, P), IsContained(R, O, P)],
      generator=sample_region),
    GenStream(inputs=[O, P, G], outputs=[Q, T],
      conditions=[IsPose(O, P), IsGrasp(O, G)],
      effects=[IsKin(O, P, G, Q, T)],
      generator=sample_motion),
    TestStream(inputs=[O, P, T],
      conditions=[IsPose(O, P)],
      effects=[IsCollisionFree(O, P, T)],
      test=collision_free)]

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

  constants = [POSE(None)]

  initial_atoms = [AtConfig(oracle.initial_config)] # TODO - toggle
  holding = set()
  if problem.start_holding is not False:
    obj, grasp = problem.start_holding
    initial_atoms += [Holding(obj, grasp), AtPose(obj, None), IsGrasp(obj, grasp)]
    holding.add(obj)
  if not holding:
    initial_atoms.append(HandEmpty())
  for obj, pose in oracle.initial_poses.iteritems():
    if obj not in holding:
      initial_atoms += [AtPose(obj, pose), IsPose(obj, pose)]

  goal_literals = []
  if problem.goal_holding is not None:
    if problem.goal_holding is False:
      goal_literals.append(HandEmpty())
    elif isinstance(problem.goal_holding, ObjGrasp):
      goal_literals.append(Holding(problem.goal_holding.object_name, problem.goal_holding.grasp))
    elif problem.goal_holding in oracle.get_objects():
      goal_literals.append(Holding(problem.goal_holding))
    else:
      raise Exception()
  for obj, pose in problem.goal_poses.iteritems():
    goal_literals.append(AtPose(obj, pose))
    initial_atoms.append(IsPose(obj, pose))
  for obj, region in problem.goal_regions.iteritems():
    goal_literals.append(InRegion(obj, region))

  goal_formula = goal_literals

  return STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)
コード例 #27
0
def compile_problem(oracle):
    problem = oracle.problem
    oracle.surface_poses = {
        surface: oracle.get_pose(surface)
        for surface in oracle.get_surfaces()
    }

    manips = []
    if ACTIVE_LEFT:
        manips.append(C('left', MANIP))
    if ACTIVE_RIGHT:
        manips.append(C('right', MANIP))

    initial_atoms = [
        AtConfig(Config(oracle.initial_config)),
    ]
    if problem.start_holding is False:  # TODO - handle if a grasp happens
        initial_atoms += [HandEmpty(manip) for manip in manips]

    #for surface in oracle.get_surfaces():
    for surface in oracle.get_counters():
        body, pose = C(surface, BODY), Pose(surface,
                                            oracle.surface_poses[surface])
        initial_atoms += [
            IsFixed(body),
            AtPose(body, pose),
            IsPose(body, pose),
        ]

    goal_literals = []
    if problem.goal_holding is not None:
        if problem.goal_holding is False:
            goal_literals += [HandEmpty(manip) for manip in manips]
        elif isinstance(problem.goal_holding, ObjGrasp):
            # TODO - add the manipulator here
            goal_literals.append(
                HasGrasp(C(problem.goal_holding.object_name, BODY),
                         C(problem.goal_holding.grasp, GRASP)))
        elif problem.goal_holding in oracle.get_objects():
            goal_literals.append(Holding(C(problem.goal_holding, BODY)))
        else:
            raise Exception()
    for obj in problem.goal_poses:
        if problem.goal_poses[obj] == 'initial':
            problem.goal_poses[obj] = oracle.initial_poses[obj]
        elif problem.goal_poses[
                obj] in oracle.initial_poses:  # Goal names other object (TODO - compile with initial)
            problem.goal_poses[obj] = oracle.initial_poses[
                problem.goal_poses[obj]]
        goal_literals.append(
            AtPose(C(obj, BODY), Pose(obj, problem.goal_poses[obj])))
        initial_atoms += [
            IsPose(C(obj, BODY), Pose(obj, problem.goal_poses[obj])
                   )  # NOTE - need to add the IsPose to everything a priori
            # TODO - stuff here
        ]
    for obj, region in problem.goal_regions.iteritems():
        goal_literals.append(InRegion(C(obj, BODY), C(region, REGION)))
    for obj, base in problem.goal_stackings.items():
        goal_literals.append(On(C(obj, BODY), C(base, BODY)))
        initial_atoms.append(AreStackable(C(obj, BODY), C(
            base, BODY)))  # NOTE - do I want to have this?
    # TODO - I guess I can handle this by making something only stackable when it needs to be
    # TODO - similarly, I could also make a condition that we only want to place in a region if it is a goal
    # TODO - make regions a type of stackable body
    # TODO - dynamically add/remove stacking
    #goal_literals = [Holding(C('blue_box', BODY))]
    goal_literals = [On(C('blue_box', BODY), C('table2', BODY))]

    goal_blocks = set()
    for atom in And(*goal_literals).get_atoms():
        for body in atom.args:
            if body.type == BODY and body.name in oracle.initial_poses:
                goal_blocks.add(body.name)
    #goal_blocks = oracle.get_objects()
    print 'Initial objects:', goal_blocks
    oracle.introduced_objects = set()
    for obj in goal_blocks:
        #for base in goal_blocks:
        #  if obj != base:
        #    initial_atoms.append(AreStackable(C(obj, BODY), C(base, BODY)))
        initial_atoms += get_object_initial_atoms(obj, oracle)

    oracle.region_supporters = {}
    for region in oracle.goal_regions:
        for surface in oracle.get_surfaces():
            if oracle.get_region(surface).region_on(oracle.get_region(region)):
                assert region not in oracle.region_supporters
                print 'Region supporter:', region, surface
                oracle.region_supporters[region] = surface
                initial_atoms.append(
                    IsRegionOn(C(region, REGION), C(surface, BODY)))
        assert region in oracle.region_supporters

    actions = [
        Move(oracle),
        Pick(oracle),
        Place(oracle),
    ]

    axioms = [
        HoldingAxiom(),
        InRegionAxiom(),
        SafeAxiom(),
        OnAxiom(),
    ]

    cond_streams = [
        PoseStream(oracle),
        GraspStream(oracle),
        ContainedStream(oracle),
        #ContainedTest(oracle),
        IKStream(oracle),
        #NearbyTest(oracle),
    ]
    if COLLISIONS:
        cond_streams.append(CollisionFreeTest(oracle))

    objects = []

    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
                              cond_streams, objects)
コード例 #28
0
def compile_problem(tamp_problem):
    O = Param(OBJECT)
    O1, O2 = Param(OBJECT), Param(OBJECT)
    L = Param(LOCATION)
    L_s, L_g = Param(LOCATION), Param(LOCATION)  # generic location
    Stove_l_s, Stove_l_g = Param(STOVE_L_S), Param(
        STOVE_L_G)  # locations for stove and sink
    Sink_l_s, Sink_l_g = Param(SINK_L_S), Param(SINK_L_G)

    actions = [
        Action(name='wash',
               parameters=[O],
               condition=And(InSink(O)),
               effect=And(Clean(O))),
        Action(name='cook',
               parameters=[O],
               condition=And(InStove(O), Clean(O)),
               effect=And(Cooked(O))),
        Action(name='pickplace',
               parameters=[O, L_s, L_g],
               condition=And(EmptySweptVolume(O, L_s, L_g), AtPose(O, L_s)),
               effect=And(AtPose(O, L_g),
                          Not(AtPose(O, L_s))))  # You should delete! 
    ]

    axioms = [
     # For all objects in the world, either object is O1 or if not, then it is not in the region
     Axiom(effect=EmptySweptVolume(O,L_s,L_g),condition=ForAll([O2],\
                                                    Or(Equal(O,O2),\
                                                    Exists([L],(And(AtPose(O2,L),OutsideRegion(O,O2,L,L_s,L_g))))))),
     Axiom(effect=InStove(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsStove(L_s,L_g)))),
     Axiom(effect=InSink(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsSink(L_s,L_g)))),
    ]

    cond_streams = [
      EasyGenStream(inputs=[O,L_s,L_g], outputs=[L], conditions=[IsSmaller(L_s,L_g)], effects=[Contained(O,L,L_s,L_g)],\
                    generator=lambda b, ls, lg: (sample_region_pose(b, ls, lg ) for _ in irange(0, INF))),
      EasyTestStream(inputs=[L_s,L_g],conditions=[],effects=[IsSmaller(L_s,L_g)],test=is_smaller,eager=EAGER_TESTS),
      EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsSink(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
      EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsStove(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
      EasyTestStream(inputs=[O,O2,L,L_s,L_g],conditions=[],effects=[OutsideRegion(O,O2,L,L_s,L_g)],test=not_in_region,eager=EAGER_TESTS),
      # OutsideRegion tests if the block at L is outside of the region (Ls,Lg)
    ]

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

    # instantiate the environment region?
    constants = [
        STOVE_L_S(tamp_problem.stove_region_s),
        STOVE_L_G(tamp_problem.stove_region_g),
        SINK_L_S(tamp_problem.sink_region_s),
        SINK_L_G(tamp_problem.sink_region_g),
    ]

    # define initial state using initial poses of objects
    initial_atoms = [
        AtPose(block, pose)
        for block, pose in tamp_problem.initial_poses.iteritems()
    ] + [IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)] + [
        IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
    ]
    # static predicate but on steroid

    # define goal state as target object to be cooked - can you infer that target object is on Stove
    goal_literals = []
    #  goal_literals.append( AtPose(tamp_problem.blue_obj,1) ) #NOTE: This works; so planner knows how to clear the area
    #  goal_literals.append( AtPose(tamp_problem.target_obj,3.0) ) #NOTE: But doing this does not work
    goal_literals.append(Cooked(tamp_problem.target_obj))
    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
                              cond_streams, constants), static_pred_names
コード例 #29
0
def solve_tamp(env):
  viewer = env.GetViewer() is not None
  problem = PROBLEM(env)

  robot = env.GetRobots()[0]
  initialize_openrave(env, ARM, min_delta=.01)
  manipulator = robot.GetActiveManipulator()
  cspace = CSpace.robot_arm(manipulator)
  base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)

  bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
  all_bodies = bodies.values()
  assert len({body.GetKinematicsGeometryHash() for body in all_bodies}) == 1 # NOTE - assuming all objects has the same geometry
  body1 = all_bodies[-1] # Generic object 1
  body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2
  grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:MAX_GRASPS]
  poses = problem.known_poses if problem.known_poses else []

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

  def _enable_all(enable): # Enables or disables all bodies for collision checking
    for body in all_bodies:
      body.Enable(enable)

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

  def cfree_pose(pose1, pose2): # Collision free test between an object at pose1 and an object at pose2
    body1.Enable(True)
    set_pose(body1, pose1.value)
    body2.Enable(True)
    set_pose(body2, pose2.value)
    return not env.CheckCollision(body1, body2)

  def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose
    _enable_all(False)
    body2.Enable(True)
    set_pose(body2, pose.value)
    for conf in traj.path():
      set_manipulator_conf(manipulator, conf)
      if env.CheckCollision(robot, body2):
        return False
    return True

  def _cfree_traj_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose
    _enable_all(False)
    body1.Enable(True)
    body2.Enable(True)
    set_pose(body2, pose.value)
    for conf in traj.path():
      set_manipulator_conf(manipulator, conf)
      manip_trans = manipulator.GetTransform()
      set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
      if env.CheckCollision(body1, body2):
        return False
    return True

  def cfree_traj(traj, pose): # Collision free test between a robot executing traj (which may or may not involve a grasp) and an object at pose
    if DISABLE_TRAJ_COLLISIONS:
      return True
    return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose))

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

  def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp
    _enable_all(False)
    body1.Enable(True)
    set_pose(body1, pose.value)
    manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
    grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) # Grasp configuration
    if grasp_conf is None: return
    if DISABLE_TRAJECTORIES:
      yield [(Conf(grasp_conf), object())]
      return

    set_manipulator_conf(manipulator, grasp_conf)
    robot.Grab(body1)
    grasp_traj = vector_traj_helper(env, robot, approach_vector) # Trajectory from grasp configuration to pregrasp
    #grasp_traj = workspace_traj_helper(base_manip, approach_vector)
    robot.Release(body1)
    if grasp_traj is None: return
    grasp_traj.grasp = grasp
    pregrasp_conf = Conf(grasp_traj.end()) # Pregrasp configuration
    yield [(pregrasp_conf, grasp_traj)]

  def sample_free_motion(conf1, conf2): # Sample motion while not holding
    if DISABLE_TRAJECTORIES:
      yield [(object(),)] # [(True,)]
      return
    _enable_all(False)
    set_manipulator_conf(manipulator, conf1.value)
    #traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
    traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
    if not traj: return
    traj.grasp = None
    yield [(traj,)]

  def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding
    if DISABLE_TRAJECTORIES:
      yield [(object(),)] # [(True,)]
      return
    _enable_all(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.grasp_trans))
    robot.Grab(body1)
    #traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
    traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
    robot.Release(body1)
    if not traj: return
    traj.grasp = grasp
    yield [(traj,)]

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

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

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

    # Collisions
    EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePose(P, P2)],
                test=cfree_pose, eager=True),
    EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)],
                test=cfree_traj),
  ]

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

  constants = map(GRASP, grasps) + map(POSE, poses)
  initial_atoms = [
    ConfEq(initial_conf),
    HandEmpty(),
  ] + [
    PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
  ]
  goal_formula = And(ConfEq(initial_conf), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
  stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)

  if viewer:
    env.UpdatePublishedBodies()
    raw_input('Start?')
  search_fn = get_fast_downward('eager', max_time=10)
  solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
  #solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=15)
  env.Lock()
  plan, universe = solve()
  env.Unlock()

  print SEPARATOR

  #universe.print_statistics()
  plan = convert_plan(plan)
  if plan is not None:
    print 'Success'
    for i, (action, args) in enumerate(plan):
      print i+1, action, args
  else:
    print 'Failure'

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

  def _execute_traj(traj):
    #for j, conf in enumerate(traj.path()):
    #for j, conf in enumerate([traj.end()]):
    path = list(sample_manipulator_trajectory(manipulator, traj.traj()))
    for j, conf in enumerate(path):
      set_manipulator_conf(manipulator, conf)
      env.UpdatePublishedBodies()
      raw_input('%s/%s) Step?'%(j, len(path)))

  if viewer and plan is not None:
    print SEPARATOR
    # Resets the initial state
    #set_manipulator_conf(manipulator, initial_conf.value)
    set_manipulator_conf(manipulator, initial_conf)
    for obj, pose in problem.initial_poses.iteritems():
      set_pose(bodies[obj], pose.value)
    env.UpdatePublishedBodies()

    if not DISABLE_TRAJECTORIES:
      for i, (action, args) in enumerate(plan):
        raw_input('\n%s/%s) Next?'%(i, len(plan)))
        if action.name == 'move':
          _, _, traj = args
          _execute_traj(traj)
        elif action.name == 'move_holding':
          _, _, traj, _, _ = args
          _execute_traj(traj)
        elif action.name == 'pick':
          obj, _, _, _, traj = args
          _execute_traj(traj.reverse())
          robot.Grab(bodies[obj])
          _execute_traj(traj)
        elif action.name == 'place':
          obj, _, _, _, traj = args
          _execute_traj(traj.reverse())
          robot.Release(bodies[obj])
          _execute_traj(traj)
        else:
          raise ValueError(action.name)
        env.UpdatePublishedBodies()
    else:
      for i, (action, args) in enumerate(plan):
        raw_input('\n%s/%s) Next?'%(i, len(plan)))
        if action.name == 'move':
          _, q2, _ = args
          set_manipulator_conf(manipulator, q2)
        elif action.name == 'move_holding':
          _, q2, _, _, _ = args
          set_manipulator_conf(manipulator, q2)
        elif action.name == 'pick':
          obj, _, _, _, traj = args
          robot.Grab(bodies[obj])
        elif action.name == 'place':
          obj, _, _, _, traj = args
          robot.Release(bodies[obj])
        else:
          raise ValueError(action.name)
        env.UpdatePublishedBodies()

  raw_input('Finish?')
コード例 #30
0
def create_problem(initRobotPos = (0.5, 0.5),
                   initRobotVar = 0.01,
                   maxMoveDist = 5.0,
                   beaconPos = (1, 1),
                   homePos = (0, 0),
                   goalPosEps = 0.1,
                   goalVar = 0.1,
                   odoErrorRate = 0.1,
                   obsVarPerDistFromSensor = 10.0,
                   minObsVar = 0.001,
                   domainSize = 20,
                   verboseFns = False):
  """
  :return: a :class:`.STRIPStreamProblem`
  """
  # Data types
  POS = Type()   # 2D position
  VAR = Type()   # 2D variance
  BEACON = Type()  # 2D position

  # Fluent predicates
  RobotPos = Pred(POS)
  RobotVar = Pred(VAR)

  # Derived predicates
  KnowYouAreHome = Pred()

  # Static predicates
  Odometry = Pred(POS, VAR, POS, VAR)
  Sensor = Pred(POS, VAR, BEACON, VAR)
  LegalPosVar = Pred(POS, VAR)
  NearbyPos = Pred(POS, POS)
  NearGoalPos = Pred(POS)
  NearGoalVar = Pred(VAR)

  # Free parameters
  RPOS1, RPOS2 = Param(POS), Param(POS)
  RVAR1, RVAR2 = Param(VAR), Param(VAR)
  BPOS = Param(BEACON)

  rename_easy(locals())

  # Helper functions
  def distance(p1, p2):
    return math.sqrt(sum([(a - b)**2 for (a,b) in zip(p1, p2)]))

  def odoVarFun(rp1, rv, rp2):
    d = distance(rp1, rp2)
    odoVar = (d * odoErrorRate)**2
    result = rv + odoVar
    if verboseFns: print 'ovf:', rv, d, result
    return [result]

  def sensorVarFun(rp, rv, bp):
    d = distance(rp, bp)
    obsVar = max(d / obsVarPerDistFromSensor, minObsVar)
    #result = round(1.0 / ((1.0 / rv) + (1.0 / obsVar)), 5) # Causes zero variance which is bad
    result = 1.0 / ((1.0 / rv) + (1.0 / obsVar))
    if verboseFns: print 'svf:', rv, d, result
    return [result]

  def randPos():
    while True:
      result = (random.random() * domainSize, random.random() * domainSize)
      print 'rp:', result
      yield [result]

  def legalTest(rp):
    (x, y) = rp
    result = (0 <= x <= domainSize) and (0 <= y <= domainSize)
    if not result: print 'not legal:', rp
    return result

  # TODO - combine variance and positions

  actions = [
    Action(name='Move',
           parameters=[RPOS1, RVAR1, RPOS2, RVAR2],
           condition = And(RobotPos(RPOS1),
                           RobotVar(RVAR1),
                           Odometry(RPOS1, RVAR1, RPOS2, RVAR2)),
           effect = And(RobotPos(RPOS2),
                        RobotVar(RVAR2),
                        Not(RobotPos(RPOS1)),
                        Not(RobotVar(RVAR1)))),

    Action(name='Look',
           parameters=[RPOS1, RVAR1, BPOS, RVAR2],
           condition = And(RobotPos(RPOS1),
                           RobotVar(RVAR1),
                           Sensor(RPOS1, RVAR1, BPOS, RVAR2)),
           effect = And(RobotVar(RVAR2),
                        Not(RobotVar(RVAR1))))
  ]

  axioms = [
  ]

  # Conditional stream declarations
  cond_streams = [
     GeneratorStream(inputs = [],
                     outputs = [RPOS1],
                     conditions = [],
                     effects = [],
                     generator = randPos),

    # TODO - the number of variances grows incredibly but we just want to consider variances possible with the move
    # Each var only has one pos because moving changes
    GeneratorStream(inputs = [RPOS1, RVAR1, RPOS2],
                    outputs = [RVAR2],
                    conditions = [LegalPosVar(RPOS1, RVAR1), NearbyPos(RPOS1, RPOS2)],
                    effects = [Odometry(RPOS1, RVAR1, RPOS2, RVAR2), LegalPosVar(RPOS2, RVAR2)],
                    generator = odoVarFun),

    GeneratorStream(inputs = [RPOS1, RVAR1, BPOS],
                    outputs = [RVAR2],
                    conditions = [LegalPosVar(RPOS1, RVAR1)],
                    effects = [Sensor(RPOS1, RVAR1, BPOS, RVAR2), LegalPosVar(RPOS1, RVAR2)],
                    generator = sensorVarFun),

    TestStream(inputs = [RPOS1, RPOS2],
               conditions = [],
               effects = [NearbyPos(RPOS1, RPOS2)],
               test = lambda rp1, rp2: distance(rp1, rp2) < maxMoveDist,
               eager = True),

    TestStream(inputs = [RPOS1],
               conditions = [],
               effects = [NearGoalPos(RPOS1)],
               test = lambda rp1: distance(rp1, homePos) < goalPosEps,
               eager = True),

    TestStream(inputs = [RVAR1],
               conditions = [],
               effects = [NearGoalVar(RVAR1)],
               test = lambda a: a < goalVar,
               eager = True)

    # NOTE - the problem seems to be the fast that this is eager

  ]

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

  constants = [
    BEACON(beaconPos),
    #POS((.1, .1)),
    #POS((3., .1)),
    #POS((6., .1)),
    POS(homePos),
  ]

  initial_atoms = [
    RobotPos(initRobotPos),
    RobotVar(initRobotVar),
    LegalPosVar(initRobotPos, initRobotVar), # TODO - I might as well keep track of pairs. Make this a legal on both
  ]

  goal_formula = Exists([RPOS1, RVAR1],
                             And(RobotPos(RPOS1),
                                 RobotVar(RVAR1),
                                 LegalPosVar(RPOS1, RVAR1),
                                 NearGoalPos(RPOS1),
                                 NearGoalVar(RVAR1)))

  problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
                                 cond_streams, constants)

  return problem