def distractions(env):
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE*np.array([-1.5, 1.5, 3]), look_point=(1.5, -1.5, 0))
obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()]
table_names = ['table1', 'table2']
regions = [
create_region(env, 'green_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 1, 0, .5))),
]
pb = place_body
center = np.array([1.75, 0.25, 0])
pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), center, 'table1')
id = count(1)
for dx, dy in product((-.15, 0, .15), repeat=2):
if not (dx == 0 and dy == 0):
pb(env, box_body(env, .07, .05, .2, name='red_box%d'%next(id), color=RED), center + np.array([dx, dy, 0]), 'table1')
#for x in irange(-.7, .7, .15):
# for y in irange(-2.0, -1.5, .15):
for x in irange(-.65, .75, .15):
for y in irange(-1.95, -1.5, .15):
pb(env, box_body(env, .05, .05, .15, name='red_box%d'%next(id), color=RED), np.array([x, y, 0]), 'table2')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]
goal_regions = {
'green_box': 'green_goal',
}
return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans,
object_names=object_names, table_names=table_names, regions=regions,
goal_regions=goal_regions)
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, 0, 1, INF, 1
elif USE_CONFIGURATION == CONFIGURATIONS.FIXED_BRANCHING:
IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = False, 1, 1, 1, 1
start_config = start_vertex.substate['robot']
holding = goal_connector.condition.substate['holding']
goal_vertex = rg.vertex(goal_connector.condition.substate)
goal_vertex.connect(goal_connector)
object_name, grasps = holding.object_name, [holding.grasp]
object_pose = start_vertex.substate[object_name]
print 'OBJECT POSE', object_pose
if object_pose is False:
print goal_connector.condition
surface_name = start_vertex.substate[(object_name, 'constrained')]
push_traj_iterator = query_to_edge_push_trajs(oracle, object_name, surface_name, object_pose, grasps)
def add_edges(n):
generated = Counter()
while int(generated) < n and not (REACHABLE_TERMINATE and goal_connector.reachable):
pap, push_traj = next(push_traj_iterator, (None, None))
if pap is None: return False
base_trajs = oracle.plan_base_roadmap(start_config, pap.approach_config)
if base_trajs is None: continue
edge = rg.edge(MovePick(object_name, pap, start_config, base_trajs, oracle))
edge.intermediates.add(edge.connectors[0])
rg.vertex(Substate({object_name: pap.pose})).connect(edge.connectors[0])
edge.connect(goal_vertex)
for push in push_traj:
base_trajs = oracle.plan_base_roadmap(start_config, push.approach_config)
if base_trajs is None: break
edge = rg.edge(MovePush(object_name, push, start_config, base_trajs, oracle))
edge.intermediates.add(edge.connectors[0])
if start_vertex.substate in edge.connectors[0].condition: start_vertex.connect(edge.connectors[0])
rg.vertex(Substate({object_name: push.start_pose})).connect(edge.connectors[0]) # NOTE - connects to pose condition
edge.connect(rg.vertex(Substate({object_name: push.end_pose})))
else:
next(generated)
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 birrt(q1, q2, distance, sample, extend, collision, restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, smooth=RRT_SMOOTHING):
for _ in irange(restarts+1):
path = rrt_connect(q1, q2, distance, sample, extend, collision, iterations=iterations)
if path is not None:
if smooth is None: return path
return smooth_path(path, extend, collision, iterations=smooth)
return None
def rrt_connect(q1, q2, distance, sample, extend, collision, iterations=RRT_ITERATIONS):
if collision(q1) or collision(q2): return None
root1, root2 = TreeNode(q1), TreeNode(q2)
nodes1, nodes2 = [root1], [root2]
for i in irange(iterations):
if len(nodes1) > len(nodes2):
nodes1, nodes2 = nodes2, nodes1
s = sample()
last1 = argmin(lambda n: distance(n.config, s), nodes1)
for q in extend(last1.config, s):
if collision(q): break
last1 = TreeNode(q, parent=last1)
nodes1.append(last1)
last2 = argmin(lambda n: distance(n.config, last1.config), nodes2)
for q in extend(last2.config, last1.config):
if collision(q): break
last2 = TreeNode(q, parent=last2)
nodes2.append(last2)
else:
path1, path2 = last1.retrace(), last2.retrace()
if path1[0] != root1:
path1, path2 = path2, path1
return configs(path1[:-1] + path2[::-1])
return None
def rrt(start, goal_sample, distance, sample, extend, collision, goal_test=lambda q: False, iterations=RRT_ITERATIONS, goal_probability=.2):
# start - start configuration
# goal - goal configuration
# sample - sampling function
# extend - extend function
# collision - collision checking fn
if collision(start): return None
if not callable(goal_sample):
g = goal_sample
goal_sample = lambda: g
nodes = [TreeNode(start)]
for i in irange(iterations):
goal = random() < goal_probability or i == 0
s = goal_sample() if goal else sample()
last = argmin(lambda n: distance(n.config, s), nodes)
for q in extend(last.config, s):
if collision(q): break
last = TreeNode(q, parent=last)
nodes.append(last)
if goal_test(last.config): return configs(last.retrace())
else:
if goal: return configs(last.retrace())
return None
def resetting_search(oracle, search_fn, max_time=INF): # TODO - allow arguments to search_fn that cap the time
start_time = time()
for i in irange(INF):
plan, state_space = search_fn()
if plan is None and time() - start_time < max_time:
print 'Resetting search %d'%i
oracle.reset()
continue
state_space.resets = i
state_space.start_time = start_time
return plan, state_space
def generator(self, start_vertex, goal_connector, rg):
oracle = self.oracle
if USE_CONFIGURATION == CONFIGURATIONS.DYNAMIC:
IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = True, 0, 1, INF, 1
elif USE_CONFIGURATION == CONFIGURATIONS.FIXED_BRANCHING:
IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = False, 1, 1, 1, 1
start_config = start_vertex.substate['robot']
object_name = goal_connector.condition.object_name
surface_name = goal_connector.condition.surface
goal_point = goal_connector.condition.point
push_traj_iterator = query_from_edge_push_trajs(oracle, object_name, surface_name, goal_point)
def add_edges(n):
generated = Counter()
while int(generated) < n and not (REACHABLE_TERMINATE and goal_connector.reachable):
pap, push_traj = next(push_traj_iterator, (None, None))
if pap is None: return False
base_trajs = oracle.plan_base_roadmap(start_config, pap.approach_config)
if base_trajs is None: continue
rg.edge(MovePlace(object_name, pap, start_config, base_trajs, oracle)).connect(
rg.vertex(Substate({object_name: pap.pose})))
for push in push_traj:
base_trajs = oracle.plan_base_roadmap(start_config, push.approach_config)
if base_trajs is None: break
edge = rg.edge(MovePush(object_name, push, start_config, base_trajs, oracle))
edge.intermediates.add(edge.connectors[0])
#if start_vertex.substate in edge.connectors[0].condition: start_vertex.connect(edge.connectors[0]) # Shouldn't ever happen
rg.vertex(Substate({object_name: push.start_pose})).connect(edge.connectors[0]) # NOTE - connects to pose condition
vertex2 = rg.vertex(Substate({object_name: push.end_pose}))
edge.connect(vertex2)
else:
vertex2.connect(goal_connector)
next(generated)
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 grow(self, goal, iterations=50, store=ts.PATH, max_tree_size=500):
if goal in self: self[goal].retrace()
if self.collision(goal): return None
nodes1, new_nodes1 = list(take(randomize(self.nodes.values()), max_tree_size)), []
nodes2, new_nodes2 = [], [TreeNode(goal)]
for _ in irange(iterations):
if len(nodes1) + len(new_nodes1) > len(nodes2) + len(new_nodes2):
nodes1, nodes2 = nodes2, nodes1
new_nodes1, new_nodes2 = new_nodes2, new_nodes1
s = self.sample()
last1 = argmin(lambda n: self.distance(n.config, s), nodes1 + new_nodes1)
for q in self.extend(last1.config, s):
if self.collision(q): break
last1 = TreeNode(q, parent=last1)
new_nodes1.append(last1)
last2 = argmin(lambda n: self.distance(n.config, last1.config), nodes2 + new_nodes2)
for q in self.extend(last2.config, last1.config):
if self.collision(q): break
last2 = TreeNode(q, parent=last2)
new_nodes2.append(last2)
else:
if len(nodes1) == 0:
nodes1, nodes2 = nodes2, nodes1
new_nodes1, new_nodes2 = new_nodes2, new_nodes1
last1, last2 = last2, last1
path1, path2 = last1.retrace(), last2.retrace()[:-1][::-1]
for p, n in pairs(path2): n.parent = p
if len(path2) == 0: # TODO - still some kind of circular error
for n in new_nodes2:
if n.parent == last2:
n.parent = path1[-1]
else:
path2[0].parent = path1[-1]
path = path1 + path2
if store in [ts.ALL, ts.SUCCESS]:
self.add(*(new_nodes1 + new_nodes2[:-1]))
elif store == ts.PATH:
new_nodes_set = set(new_nodes1 + new_nodes2[:-1])
self.add(*[n for n in path if n in new_nodes_set])
return path
if store == ts.ALL:
self.add(*new_nodes1)
return None
def reset_reachability_generator(vertex, oracle, rg_fn, h_fn, ha_fn, only_initial=None, initial_reset_time=INF,
reset_time=INF, reset_iterations=INF, reset_cycles=INF, max_time=INF):
start_time = time()
is_initial = only_initial is not None and vertex.state == only_initial
rg = rg_fn(vertex.state)
for i in irange(INF):
if rg.exhausted: break
rg.grow(max_time=(initial_reset_time if is_initial else reset_time), max_iterations=reset_iterations,
max_cycles=reset_cycles, greedy=True)
if rg.reachable:
yield h_fn(rg), ha_fn(rg)
elif time() - start_time < max_time and (only_initial is None or is_initial):
print 'Resetting reachability graph %d'%i
oracle.reset() # TODO - for some reason this resets instantaneously several times
rg = rg_fn(vertex.state)
else:
yield INF if not rg.exhausted else None, []
def grow(self, goal_sample, iterations=50, goal_probability=.2, store=ts.PATH, max_tree_size=500):
if not callable(goal_sample): goal_sample = lambda: goal_sample
nodes, new_nodes = list(take(randomize(self.nodes.values()), max_tree_size)), []
for i in irange(iterations):
goal = random() < goal_probability or i == 0
s = goal_sample() if goal else self.sample()
last = argmin(lambda n: self.distance(n.config, s), nodes + new_nodes)
for q in self.extend(last.config, s):
if self.collision(q): break
last = TreeNode(q, parent=last)
new_nodes.append(last)
else:
if goal:
path = last.retrace()
if store in [ts.ALL, ts.SUCCESS]:
self.add(*new_nodes)
elif store == ts.PATH:
new_nodes_set = set(new_nodes)
self.add(*[n for n in path if n in new_nodes_set])
return path
if store == ts.ALL:
self.add(*new_nodes)
return None
