def create_problems(args): task_fn_from_name = {fn.__name__: fn for fn in TASKS_FNS} problems = [] for num in range(N_TRIALS): for task_name in TASK_NAMES: print('Trial: {} / {} | Task: {}'.format(num, N_TRIALS, task_name)) random.seed(hash((0, task_name, num, time.time()))) numpy.random.seed( wrap_numpy_seed(hash((1, task_name, num, time.time())))) world = World(use_gui=False) # SERIAL task_fn = task_fn_from_name[task_name] task = task_fn(world, fixed=args.fixed) task.world = None if not SERIALIZE_TASK: task = task_name saver = WorldSaver() problems.append({ 'task': task, 'trial': num, 'saver': saver, #'seeds': [get_random_seed(), get_numpy_seed()], #'seeds': [random.getstate(), numpy.random.get_state()], }) #print(world.body_from_name) # TODO: does not remain the same #wait_for_user() #world.reset() #if has_gui(): # wait_for_user() world.destroy() return problems
def main(): parser = argparse.ArgumentParser() #parser.add_argument('-attempts', default=100, type=int, # help='The number of attempts') parser.add_argument('-cfree', action='store_true', help='When enabled, disables collision checking (for debugging).') #parser.add_argument('-grasp_type', default=GRASP_TYPES[0], # help='Specifies the type of grasp.') #parser.add_argument('-problem', default='test_block', # help='The name of the problem to solve.') parser.add_argument('-max_time', default=10*60, type=float, help='The maximum runtime') parser.add_argument('-num_samples', default=1000, type=int, help='The number of samples') parser.add_argument('-robot', default=FRANKA_CARTER, choices=[FRANKA_CARTER, EVE], help='The robot to use.') parser.add_argument('-seed', default=None, help='The random seed to use.') parser.add_argument('-teleport', action='store_true', help='Uses unit costs') parser.add_argument('-visualize', action='store_true', help='When enabled, visualizes planning rather than the world (for debugging).') args = parser.parse_args() world = World(use_gui=args.visualize, robot_name=args.robot) #dump_body(world.robot) for joint in world.kitchen_joints: world.open_door(joint) # open_door | close_door world.open_gripper() # TODO: sample from set of objects? object_name = '{}_{}_block{}'.format(BLOCK_SIZES[-1], BLOCK_COLORS[0], 0) world.add_body(object_name) # TODO: could constrain Eve to be within a torso cone grasp_colors = { TOP_GRASP: RED, SIDE_GRASP: BLUE, } #combinations = list(product(OPEN_SURFACES, GRASP_TYPES)) \ # + [(surface_name, TOP_GRASP) for surface_name in DRAWERS] \ # + [(surface_name, SIDE_GRASP) for surface_name in CABINETS] # ENV_SURFACES combinations = [] for surface_name in ZED_LEFT_SURFACES: if surface_name in (OPEN_SURFACES + DRAWERS): combinations.append((surface_name, TOP_GRASP)) if surface_name in (OPEN_SURFACES + CABINETS): combinations.append((surface_name, SIDE_GRASP)) # TODO: parallelize print('Combinations:', combinations) wait_for_user('Start?') for surface_name, grasp_type in combinations: #draw_picks(world, object_name, surface_name, grasp_type, color=grasp_colors[grasp_type]) collect_place(world, object_name, surface_name, grasp_type, args) world.destroy()
def main(): parser = argparse.ArgumentParser() #parser.add_argument('-attempts', default=100, type=int, # help='The number of attempts') parser.add_argument( '-cfree', action='store_true', help='When enabled, disables collision checking (for debugging).') parser.add_argument('-max_time', default=10 * 60, type=float, help='The maximum runtime') parser.add_argument('-num_samples', default=1000, type=int, help='The number of samples') parser.add_argument('-seed', default=None, help='The random seed to use.') parser.add_argument('-teleport', action='store_true', help='Uses unit costs') parser.add_argument( '-visualize', action='store_true', help= 'When enabled, visualizes planning rather than the world (for debugging).' ) args = parser.parse_args() # TODO: could record the full trajectories here world = World(use_gui=args.visualize) world.open_gripper() #joint_names = DRAWER_JOINTS + CABINET_JOINTS joint_names = ZED_LEFT_JOINTS print('Joints:', joint_names) print('Knobs:', KNOBS) wait_for_user('Start?') for joint_name in joint_names: collect_pull(world, joint_name, args) for knob_name in KNOBS: collect_pull(world, knob_name, args) world.destroy()
def main(): task_names = [fn.__name__ for fn in TASKS_FNS] print('Tasks:', task_names) parser = create_parser() parser.add_argument('-problem', default=task_names[-1], choices=task_names, help='The name of the problem to solve.') parser.add_argument('-record', action='store_true', help='When enabled, records and saves a video at {}'.format( VIDEO_TEMPLATE.format('<problem>'))) args = parser.parse_args() #if args.seed is not None: # set_seed(args.seed) #set_random_seed(0) # Doesn't ensure deterministic #set_numpy_seed(1) print('Random seed:', get_random_seed()) print('Numpy seed:', get_numpy_seed()) np.set_printoptions(precision=3, suppress=True) world = World(use_gui=True) task_fn_from_name = {fn.__name__: fn for fn in TASKS_FNS} task_fn = task_fn_from_name[args.problem] task = task_fn(world, num=args.num, fixed=args.fixed) wait_for_duration(0.1) world._update_initial() print('Objects:', task.objects) #target_point = get_point(world.get_body(task.objects[0])) #set_camera_pose(camera_point=target_point+np.array([-1, 0, 1]), target_point=target_point) #if not args.record: # with LockRenderer(): # add_markers(task, inverse_place=False) #wait_for_user() # TODO: FD instantiation is slightly slow to a deepcopy # 4650801/25658 2.695 0.000 8.169 0.000 /home/caelan/Programs/srlstream/pddlstream/pddlstream/algorithms/skeleton.py:114(do_evaluate_helper) #test_observation(world, entity_name='big_red_block0') #return # TODO: mechanism that pickles the state of the world real_state = create_state(world) video = None if args.record: wait_for_user('Start?') video_path = VIDEO_TEMPLATE.format(args.problem) video = VideoSaver(video_path) time_step = None if args.teleport else DEFAULT_TIME_STEP def observation_fn(belief): return observe_pybullet(world) def transition_fn(belief, commands): # if not args.record: # Video doesn't include planning time # wait_for_user() # restore real_state just in case? # wait_for_user() #if args.fixed: # args.simulate return simulate_commands(real_state, commands) #return iterate_commands(real_state, commands, time_step=time_step, pause=False) run_policy(task, args, observation_fn, transition_fn) if video: print('Saved', video_path) video.restore() world.destroy()
def run_experiment(experiment): problem = experiment['problem'] task_name = problem['task'].name if SERIALIZE_TASK else problem['task'] trial = problem['trial'] policy = experiment['policy'] set_memory_limits() if not VERBOSE: sys.stdout = open(os.devnull, 'w') stdout = sys.stdout if not SERIAL: current_wd = os.getcwd() # trial_wd = os.path.join(current_wd, TEMP_DIRECTORY, '{}/'.format(os.getpid())) trial_wd = os.path.join( current_wd, TEMP_DIRECTORY, 't={}_n={}_{}/'.format(task_name, trial, name_from_policy(policy))) safe_rm_dir(trial_wd) ensure_dir(trial_wd) os.chdir(trial_wd) parser = create_parser() args = parser.parse_args() task_fn_from_name = {fn.__name__: fn for fn in TASKS_FNS} task_fn = task_fn_from_name[task_name] world = World(use_gui=SERIAL) if SERIALIZE_TASK: task_fn(world, fixed=args.fixed) task = problem['task'] world.task = task task.world = world else: # TODO: assumes task_fn is deterministic wrt task task_fn(world, fixed=args.fixed) problem['saver'].restore() world._update_initial() problem['task'] = task_name # for serialization del problem['saver'] random.seed(hash((0, task_name, trial, time.time()))) numpy.random.seed(hash((1, task_name, trial, time.time())) % (2**32)) #seed1, seed2 = problem['seeds'] # No point unless you maintain the same random state per generator #set_random_seed(seed1) #set_random_seed(seed2) #random.setstate(state1) #numpy.random.set_state(state2) reset_globals() real_state = create_state(world) #start_time = time.time() #if has_gui(): # wait_for_user() observation_fn = lambda belief: observe_pybullet(world) transition_fn = lambda belief, commands: iterate_commands( real_state, commands, time_step=0) outcome = dict(ERROR_OUTCOME) try: with timeout(MAX_TIME + TIME_BUFFER): outcome = run_policy(task, args, observation_fn, transition_fn, max_time=MAX_TIME, **policy) outcome['error'] = False except KeyboardInterrupt: raise KeyboardInterrupt() except: traceback.print_exc() #outcome = {'error': True} world.destroy() if not SERIAL: os.chdir(current_wd) safe_rm_dir(trial_wd) if not VERBOSE: sys.stdout.close() sys.stdout = stdout result = { 'experiment': experiment, 'outcome': outcome, } return result