コード例 #1
0
  def generator(self, start_vertex, goal_connector, rg):
    oracle = self.oracle
    if USE_CONFIGURATION == CONFIGURATIONS.DYNAMIC:
      IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = True, 2, 1, INF, 1
    elif USE_CONFIGURATION == CONFIGURATIONS.FIXED_BRANCHING:
      IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = False, 2, 1, 1, 1
    NUM_CANDIDATE_POSES = 5

    start_config = start_vertex.substate['robot']
    if isinstance(goal_connector.condition, HoldingCondition):
      object_name = goal_connector.condition.object_name
      grasps = get_grasps(oracle, object_name)
    else:
      holding = goal_connector.condition.substate['holding']
      object_name, grasps = holding.object_name, [holding.grasp]
    preferred_poses = [start_vertex.substate[object_name]] if start_vertex.substate[object_name] is not False else []

    pap_iterator = list(flatten(existing_paps(oracle, object_name, grasps=grasps, poses=preferred_poses))) # NOTE - avoids repeated tries
    if len(pap_iterator) == 0:
      pap_iterator = chain(query_paps(oracle, object_name, grasps=grasps, poses=preferred_poses,
          object_poses=state_obstacles(object_name, start_vertex.substate, oracle), max_failures=15), # start pose & objects
        query_paps(oracle, object_name, grasps=grasps, poses=preferred_poses, max_failures=25), # start pose
        query_paps(oracle, object_name, grasps=grasps, num_poses=NUM_CANDIDATE_POSES,
          object_poses=state_obstacles(object_name, start_vertex.substate, oracle), max_failures=10)) # any pose & objects
        #query_paps(oracle, object_name, grasps=[grasp], num_poses=NUM_POSES)) # any pose
    else:
      pap_iterator = iter(pap_iterator)

    def add_edges(n):
      generated = Counter()
      while int(generated) < n and not (REACHABLE_TERMINATE and goal_connector.reachable):
        pap = next(pap_iterator, (None, None))[0]
        if pap is None: return False
        base_trajs = oracle.plan_base_roadmap(start_config, pap.approach_config)
        if base_trajs is None: continue
        condition_vertex = rg.vertices.get(Substate({object_name: pap.pose}), None) # Avoids circular poses
        if not (condition_vertex is None or condition_vertex.reachable or not condition_vertex.active): continue
        #if condition_vertex is None or condition_vertex.reachable or not condition_vertex.active: next(generated)
        next(generated)
        goal_vertex = rg.vertex(Substate({'holding': Holding(object_name, pap.grasp)}))
        goal_vertex.connect(goal_connector)
        rg.edge(MovePick(object_name, pap, start_config, base_trajs, oracle)).connect(goal_vertex)
      return True

    if not IMMEDIATE: yield
    for calls in irange(MAX_CALLS):
      if PRINT: print '%s-%s: %d'%(self.__class__.__name__, goal_connector.condition, calls)
      if not add_edges(PER_CALL if calls!=0 else INITIAL): break
      for _ in irange(RECURRENCE): yield
コード例 #2
0
  def generator(self, start_vertex, goal_connector, rg):
    oracle = self.oracle
    if USE_CONFIGURATION == CONFIGURATIONS.DYNAMIC:
      IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = True, 1, 1, INF, 1
    elif USE_CONFIGURATION == CONFIGURATIONS.FIXED_BRANCHING:
      IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = False, 4, 1, 1, 1

    start_config = start_vertex.substate['robot']
    object_name = goal_connector.condition.object_name
    start_pose = start_vertex.substate[object_name]

    goal_vertex = rg.vertex(Substate({object_name: False}))
    goal_vertex.connect(goal_connector)

    pap_iterator = list(flatten(existing_paps(oracle, object_name, poses=[start_pose], num_grasps=INF))) # NOTE - avoids repeated tries
    if len(pap_iterator) == 0:
      pap_iterator = chain(query_paps(oracle, object_name, poses=[start_pose],
          num_grasps=INF, object_poses=state_obstacles(object_name, start_vertex.substate, oracle), max_failures=15),
        query_paps(oracle, object_name, poses=[start_pose], num_grasps=INF, max_failures=25))
    else:
      pap_iterator = iter(pap_iterator)

    def add_edges(n):
      generated = Counter()
      while int(generated) < n and not (REACHABLE_TERMINATE and goal_connector.reachable):
        pap = next(pap_iterator, (None, None))[0]
        if pap is None: return False
        base_trajs = oracle.plan_base_roadmap(start_config, pap.approach_config)
        if base_trajs is None: continue
        next(generated)
        rg.edge(MovePick(object_name, pap, start_config, base_trajs, oracle)).connect(goal_vertex)
      return True

    if not IMMEDIATE: yield
    for calls in irange(MAX_CALLS):
      if PRINT: print '%s-%s: %d'%(self.__class__.__name__, goal_connector.condition, calls)
      if not add_edges(PER_CALL if calls!=0 else INITIAL): break
      for _ in irange(RECURRENCE): yield