def create_custom_ir(robot, manip_base_iterator, n=IR_DATABASE_SAMPLES):
if DEBUG: print 'Creating inverse reachability database'
ir_database = []
for manip_trans, base_trans in take(manip_base_iterator, n):
manip_trans2D = trans_from_base_values([manip_trans[0, 3], manip_trans[1, 3], get_manip_angle(robot, manip_trans)])
base_trans2D = trans2D_from_trans(base_trans)
ir_database.append(base_values_from_trans(np.linalg.solve(manip_trans2D, base_trans2D))) # M * T = B
return ir_database
def pap_ir_samples(env, max_failures=100, max_attempts=INF): # NOTE - max_failures should be large to prevent just easy placements
from manipulation.inverse_reachability.inverse_reachability import openrave_base_iterator, create_custom_ir
from manipulation.pick_and_place import PickAndPlace
oracle = pap_ir_oracle(env)
body_name = oracle.objects[0]
table_name = oracle.tables[0]
for pose in take(random_region_placements(oracle, body_name, [table_name]), max_attempts):
grasp = choice(get_grasps(oracle, body_name))
pap = PickAndPlace(None, pose, grasp)
if pap.sample(oracle, body_name, base_iterator_fn=openrave_base_iterator,
max_failures=max_failures, check_base=False):
yield pap.manip_trans, pap.base_trans
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 pap_ir_statistics(env, trials=100):
from manipulation.inverse_reachability.inverse_reachability import display_custom_ir
from manipulation.pick_and_place import PickAndPlace
oracle = pap_ir_oracle(env)
body_name = oracle.objects[0]
table_name = oracle.tables[0]
successes = []
for pose in take(random_region_placements(oracle, body_name, [table_name]), trials):
grasp = choice(get_grasps(oracle, body_name))
pap = PickAndPlace(None, pose, grasp)
oracle.set_pose(body_name, pap.pose)
#if pap.sample(oracle, body_name, max_failures=50, base_iterator_fn=openrave_base_iterator, check_base=False):
if pap.sample(oracle, body_name, max_failures=50, check_base=False):
oracle.set_robot_config(pap.grasp_config)
successes.append(int(pap.iterations))
handles = display_custom_ir(oracle, pap.manip_trans)
raw_input('Continue?')
return float(len(successes))/trials, np.mean(successes), np.std(successes)
def srivastava_table(env, n=INF):
env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae')
set_default_robot_config(env.GetRobots()[0])
body_names = [get_name(body) for body in env.GetBodies() if not body.IsRobot()]
table_names = [body_name for body_name in body_names if 'table' in body_name]
dx = .5
for body_name in body_names:
body = env.GetKinBody(body_name)
set_point(body, get_point(body) + np.array([dx, 0, 0]))
objects = [env.GetKinBody(body_name) for body_name in body_names if body_name not in table_names]
for obj in objects: env.Remove(obj)
object_names = []
for obj in take(objects, n):
randomly_place_body(env, obj, table_names)
object_names.append(get_name(obj))
goal_holding = 'object1'
goal_config = 'initial' # None
return ManipulationProblem(None,
object_names=object_names, table_names=table_names,
goal_config=goal_config, goal_holding=goal_holding)
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
