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
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
def convex_hull_vertices(hull): # NOTE - old scipy uses points while the new scipy uses vertices
if not hasattr(hull, "vertices"):
return sorted(set(flatten(hull.simplices)))
return hull.vertices
def merge(*roadmaps): new_roadmap = Roadmap() new_roadmap.vertices = merge_dicts(*[roadmap.vertices for roadmap in roadmaps]) new_roadmap.edges = list(flatten(roadmap.edges for roadmap in roadmaps)) return new_roadmap
