def sample_start_paps(oracle):
if DEBUG: print function_name(inspect.stack())
important_objects = set(oracle.problem.goal_regions.keys() + oracle.problem.goal_poses.keys())
oracle.reachable_objects = set()
for object_name in oracle.get_objects():
max_failures = N_START_FAILURES if object_name not in important_objects else N_IMPORTANT_START_FAILURES
if sample_pose(oracle, object_name, oracle.initial_poses[object_name], num_grasps=NUM_GRASPS, max_failures=max_failures):
oracle.reachable_objects.add(object_name)
else:
#set_color(oracle.get_body(object_name), np.array([0, 0, 0, 1])) # TODO - remove
if DEBUG: print object_name + ' is not reachable'
def sample_goal_pose_paps(oracle):
if DEBUG: print function_name(inspect.stack())
for object_name, object_pose in oracle.problem.goal_poses.items():
if object_name in oracle.reachable_objects:
sample_pose(oracle, object_name, object_pose, num_grasps=NUM_GRASPS, max_failures=NUM_GOAL_POSE_FAILURES)