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)
