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 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)