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))
class StreamFn(FunctionStream
): # TODO - maybe find a better way of doing this?
num_samples = 2
avoid_initial = True
max_failures = 20 # INF
# TODO - make the cost of the stream depend on the object below
def function(self, (b, bs, ps)):
oracle = self.cond_stream.oracle
with oracle.state_saver():
if self.avoid_initial:
oracle.set_all_object_poses(oracle.initial_poses)
else:
oracle.set_all_object_poses(
{b.name: oracle.initial_poses[b.name]})
if bs.name in oracle.get_counters():
poses = random_region_placements(
oracle,
b.name, [bs.name],
region_weights=True,
max_failures=self.max_failures)
#poses = cached_region_placements(self.oracle, self.obj, [bs.name], order=None, random=True)
else:
poses = center_stackings(oracle,
b.name, [(bs.name, ps.value)],
max_failures=self.max_failures)
return [(Pose(b.name, pose), )
for pose in islice(poses, self.num_samples)]
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]
num_samples = Counter()
for i, pose in enumerate(
take(random_region_placements(oracle, body_name, [table_name]),
max_attempts)):
if i % 10 == 0:
print 'IR sampling iteration:', i, '| samples:', num_samples
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
next(num_samples)
class StreamFn(FunctionStream
): # TODO - maybe find a better way of doing this?
num_samples = 2
avoid_initial = True
max_failures = 20 # INF
def function(self, (b, bs)):
oracle = self.cond_stream.oracle
with oracle.state_saver():
if self.avoid_initial:
oracle.set_all_object_poses(oracle.initial_poses)
else:
oracle.set_all_object_poses(
{b.name: oracle.initial_poses[b.name]})
assert bs.name in oracle.surface_poses
poses = random_region_placements(
oracle,
b.name, [bs.name],
region_weights=True,
max_failures=self.max_failures)
#poses = cached_region_placements(self.oracle, self.obj, self.oracle.get_counters(), order=None, random=True)
r_pose = oracle.surface_poses[bs.name]
combos = []
for pose in islice(poses, self.num_samples):
rel_pose = utils.Pose(
pose_from_trans(
np.dot(
trans_from_pose(pose.value),
np.linalg.inv(trans_from_pose(
r_pose.value))))) # p = r * t
combos.append((RelPose((b.name, rel_pose, bs.name)), ))
return combos
def sample_region(o, r, num_samples=1):
while True:
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
poses = random_region_placements(oracle, o, [r], region_weights=True)
fixed_poses = []
for pose in islice(poses, num_samples):
pose.obj = o
fixed_poses.append(pose)
yield fixed_poses
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))
class StreamFn(FunctionStream
): # TODO - maybe find a better way of doing this?
num_samples = 2
def function(self, (b, r)): # TODO - switch this to be a generator
poses = random_region_placements(
self.cond_stream.oracle, b.name, [r.name],
region_weights=True) # TODO - save this generator
return [(Pose(b.name, pose), )
for pose in islice(poses, self.num_samples)]
def sample_pose(b):
with oracle.state_saver():
if AVOID_INITIAL:
oracle.set_all_object_poses(oracle.initial_poses)
else:
oracle.set_all_object_poses({b: oracle.initial_poses[b]})
poses = random_region_placements(oracle, b, oracle.get_counters(), region_weights=True)
#poses = cached_region_placements(self.oracle, self.obj, self.oracle.get_counters(), order=None, random=True)
fixed_poses = []
for pose in islice(poses, NUM_POSES):
pose.obj = b
#yield pose
fixed_poses.append(pose)
yield fixed_poses
def sample_pose(o, num_samples=2):
#with oracle.state_saver():
while True:
if AVOID_INITIAL:
oracle.set_all_object_poses(oracle.initial_poses)
else:
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
poses = random_region_placements(oracle, o, oracle.get_counters(), region_weights=True)
#poses = cached_region_placements(self.oracle, self.obj, self.oracle.get_counters(), order=None, random=True)
fixed_poses = []
for pose in islice(poses, num_samples):
pose.obj = o
#yield pose
fixed_poses.append(pose)
yield fixed_poses
class StreamFn(FunctionStream
): # TODO - maybe find a better way of doing this?
num_samples = 2
def function(self, (b, )):
oracle = self.cond_stream.oracle
#with oracle.state_saver():
#if avoid_initial: oracle.set_all_object_poses(oracle.initial_poses)
#else: oracle.set_all_object_poses({object_name: oracle.initial_poses[object_name]})
#counter = Counter()
poses = random_region_placements(oracle,
b.name,
oracle.get_counters(),
region_weights=True)
#poses = cached_region_placements(self.oracle, self.obj, self.oracle.get_counters(), order=None, random=True)
return [(Pose(b.name, pose), )
for pose in islice(poses, self.num_samples)]
def sample_region(o, r, max_samples=2):
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
poses = random_region_placements(oracle, o, [r], region_weights=True)
for pose in islice(poses, max_samples):
pose.obj = o
yield [pose]
def pap_ir_statistics(env, trials=100):
from manipulation.inverse_reachability.inverse_reachability import display_custom_ir, load_ir_database, \
ir_base_trans, forward_transform, manip_base_values, is_possible_fr_trans, is_possible_ir_trans
from manipulation.pick_and_place import PickAndPlace
oracle = pap_ir_oracle(env)
body_name = oracle.objects[0]
table_name = oracle.tables[0]
convert_point = lambda p: np.concatenate([p[:2], [1.]])
"""
load_ir_database(oracle)
print oracle.ir_aabb
print aabb_min(oracle.ir_aabb), aabb_max(oracle.ir_aabb)
handles = []
vertices = xy_points_from_aabb(oracle.ir_aabb)
print vertices
print np.min(oracle.ir_database, axis=0), np.max(oracle.ir_database, axis=0)
for v in vertices:
handles.append(draw_point(oracle.env, convert_point(v)))
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(draw_line(oracle.env, convert_point(v1), convert_point(v2)))
raw_input('Start?')
"""
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)
default_trans = get_trans(oracle.robot)
#vertices = xy_points_from_aabb(aabb_apply_trans(oracle.ir_aabb, pap.manip_trans))
vertices = [
point_from_trans(
ir_base_trans(oracle.robot, pap.manip_trans, v, default_trans))
for v in xy_points_from_aabb(oracle.ir_aabb)
]
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(
draw_line(oracle.env, convert_point(v1), convert_point(v2)))
point_from_inverse = lambda trans: point_from_trans(
np.dot(pap.base_trans, forward_transform(trans)))
for trans in oracle.ir_database:
handles.append(
draw_point(oracle.env,
convert_point(point_from_inverse(trans)),
color=(0, 1, 0, .5)))
#vertices = [point_from_inverse(v) for v in xy_points_from_aabb(oracle.ir_aabb)] # Doesn't have the angle in it...
vertices = [
point_from_trans(np.dot(pap.base_trans, trans_from_base_values(v)))
for v in xy_points_from_aabb(oracle.fr_aabb)
]
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(
draw_line(oracle.env,
convert_point(v1),
convert_point(v2),
color=(0, 1, 0, .5)))
assert is_possible_ir_trans(oracle, pap.manip_trans, pap.base_trans) and \
is_possible_fr_trans(oracle, pap.base_trans, pap.manip_trans)
raw_input('Continue?')
return float(
len(successes)) / trials, np.mean(successes), np.std(successes)
def sample_region(o, r, max_samples=2):
poses = random_region_placements(oracle, o, [r], region_weights=True)
for pose in islice(poses, max_samples):
pose.obj = o
yield [pose]
