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]