def main(display='execute'): # control | execute | step connect(use_gui=True) disable_real_time() robot = load_model(DRAKE_IIWA_URDF) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor)))) set_default_camera() dump_world() saved_world = WorldSaver() command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') return saved_world.restore() update_state() user_input('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.005) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_for_interrupt() disconnect()
def test_arm_control(pr2, left_joints, arm_start): user_input('Control Arm?') real_time = False enable_gravity() p.setRealTimeSimulation(real_time) for _ in joint_controller(pr2, left_joints, arm_start): if not real_time: p.stepSimulation()
def main(): connect(use_gui=True) with HideOutput(): pr2 = load_model("models/pr2_description/pr2.urdf") test_clone_robot(pr2) test_clone_arm(pr2) user_input('Finish?') disconnect()
def test_arm_motion(pr2, left_joints, arm_goal): disabled_collisions = get_disabled_collisions(pr2) user_input('Plan Arm?') arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions) if arm_path is None: print('Unable to find an arm path') return print(len(arm_path)) for q in arm_path: set_joint_positions(pr2, left_joints, q) #raw_input('Continue?') time.sleep(0.01)
def main(): # collect data in parallel, parameters are generated uniformly randomly in a range # data stored to pour_date/trails_n=10000.json # TODO: ulimit settings # https://ss64.com/bash/ulimit.html # https://stackoverflow.com/questions/938733/total-memory-used-by-python-process # import psutil # TODO: resource.getrusage(resource.RUSAGE_SELF).ru_maxrss assert (get_python_version() == 3) parser = argparse.ArgumentParser() parser.add_argument('-f', '--fn', default=TRAINING, help='The parameter function to use.') parser.add_argument('-n', '--num', type=int, default=10000, help='The number of samples to collect.') parser.add_argument('-p', '--problem', required=True, choices=sorted(SKILL_COLLECTORS.keys()), help='The name of the skill to learn.') parser.add_argument('-t', '--time', type=int, default=1 * 60, help='The max planning runtime for each trial.') parser.add_argument('-v', '--visualize', action='store_true', help='When enabled, visualizes execution.') args = parser.parse_args() serial = is_darwin() assert implies(args.visualize, serial) trials = get_trials(args.problem, args.fn, args.num, max_time=args.time, valid=True, visualize=args.visualize, verbose=serial) data_path = None if serial else get_data_path(args.problem, trials) num_cores = get_num_cores(trials, serial) user_input('Begin?') # TODO: store the generating distribution for samples and objects? print(SEPARATOR) results = run_trials(trials, data_path, num_cores=num_cores)
def test_base_motion(pr2, base_start, base_goal): #disabled_collisions = get_disabled_collisions(pr2) set_base_values(pr2, base_start) user_input('Plan Base?') base_limits = ((-2.5, -2.5), (2.5, 2.5)) base_path = plan_base_motion(pr2, base_goal, base_limits) if base_path is None: print('Unable to find a base path') return print(len(base_path)) for bq in base_path: set_base_values(pr2, bq) user_input('Continue?')
def fn(body, pose, grasp): obstacles = [body] + fixed gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose) approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose) draw_pose(gripper_pose, length=0.04) draw_pose(approach_pose, length=0.04) for _ in range(num_attempts): if USE_IKFAST: q_approach = sample_tool_ik(robot, approach_pose) if q_approach is not None: set_joint_positions(robot, movable_joints, q_approach) else: set_joint_positions(robot, movable_joints, sample_fn()) # Random seed q_approach = inverse_kinematics(robot, grasp.link, approach_pose) if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles): continue conf = BodyConf(robot, joints=movable_joints) if USE_IKFAST: q_grasp = sample_tool_ik(robot, gripper_pose, closest_only=True) if q_grasp is not None: set_joint_positions(robot, movable_joints, q_grasp) else: q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose) if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles): continue if teleport: path = [q_approach, q_grasp] else: conf.assign() #direction, _ = grasp.approach_pose #path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction, # quat_from_pose(approach_pose)) path = plan_direct_joint_motion(robot, conf.joints, q_grasp, obstacles=obstacles, \ self_collisions=self_collisions) if path is None: if DEBUG_FAILURE: user_input('Approach motion failed') continue command = Command([BodyPath(robot, path, joints=movable_joints), Attach(body, robot, grasp.link), BodyPath(robot, path[::-1], joints=movable_joints, attachments=[grasp])]) return (conf, command) # TODO: holding collisions return None
def main(use_pr2_drake=False): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") #table_path = "table/table.urdf" # table_path = "models/table_collision/table.urdf" # table = p.loadURDF(table_path, 0, 0, 0, 0, 0, 0.707107, 0.707107) # table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF with HideOutput(): pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings? dump_body(pr2) base_start = (-2, -2, 0) base_goal = (2, 2, 0) arm_start = SIDE_HOLDING_LEFT_ARM #arm_start = TOP_HOLDING_LEFT_ARM #arm_start = REST_LEFT_ARM arm_goal = TOP_HOLDING_LEFT_ARM #arm_goal = SIDE_HOLDING_LEFT_ARM left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) set_joint_positions(pr2, left_joints, arm_start) set_joint_positions(pr2, right_joints, rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, torso_joints, [0.2]) open_arm(pr2, 'left') # test_ikfast(pr2) p.addUserDebugLine(base_start, base_goal, lineColorRGB=(1, 0, 0)) # addUserDebugText print(base_start, base_goal) if use_pr2_drake: test_drake_base_motion(pr2, base_start, base_goal) else: test_base_motion(pr2, base_start, base_goal) test_arm_motion(pr2, left_joints, arm_goal) # test_arm_control(pr2, left_joints, arm_start) user_input('Finish?') disconnect()
def test_drake_base_motion(pr2, base_start, base_goal): # TODO: combine this with test_arm_motion """ Drake's PR2 URDF has explicit base joints """ disabled_collisions = get_disabled_collisions(pr2) base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']] set_joint_positions(pr2, base_joints, base_start) user_input('Plan Base?') base_path = plan_joint_motion(pr2, base_joints, base_goal, disabled_collisions=disabled_collisions) if base_path is None: print('Unable to find a base path') return print(len(base_path)) for bq in base_path: set_joint_positions(pr2, base_joints, bq) user_input('Continue?')
def main(display='execute'): # control | execute | step connect(use_gui=True) disable_real_time() with HideOutput(): root_directory = os.path.dirname(os.path.abspath(__file__)) robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True) floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) floor_x = 2 set_pose(floor, Pose(Point(x=floor_x, z=0.5))) set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor)))) # set_default_camera() dump_world() saved_world = WorldSaver() with LockRenderer(): command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') print('Quit?') wait_for_interrupt() disconnect() return saved_world.restore() update_state() user_input('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.002) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_for_interrupt() disconnect()
wait_for_duration, enable_gravity, enable_real_time, trajectory_controller, simulate_controller, \ add_fixed_constraint, remove_fixed_constraint, Pose, Euler, get_collision_fn, LockRenderer, user_input, has_gui, \ disconnect from pddlstream.algorithms.focused import solve_focused from pddlstream.language.constants import And, print_solution from pddlstream.language.generator import from_gen_fn, from_fn from pddlstream.utils import read, get_file_path try: from conrob_pybullet.utils.ikfast.kuka_kr6_r900.ik import sample_tool_ik except ImportError as e: print('\x1b[6;30;43m' + '{}, Using pybullet ik fn instead'.format(e) + '\x1b[0m') USE_IKFAST = False user_input("Press Enter to continue...") else: USE_IKFAST = True PICKNPLACE_DIRECTORY = os.path.join('..', 'assembly_instances', 'picknplace') PICKNPLACE_FILENAMES = { #'choreo_brick_demo': 'choreo_brick_demo.json', 'choreo_brick_demo': 'json/brick_demo.json', 'choreo_eth-trees_demo': 'choreo_eth-trees_demo.json', } GRASP_NAMES = [ 'pick_grasp_approach_plane', 'pick_grasp_plane', 'pick_grasp_retreat_plane' ] TOOL_NAME = 'eef_tcp_frame' # robot_tool0 | eef_base_link | eef_tcp_frame SELF_COLLISIONS = False MILLIMETER = 0.001
def solve_serialized(robots, obstacles, node_points, element_bodies, ground_nodes, layer_from_n, trajectories=[], post_process=False, collisions=True, disable=False, max_time=INF, **kwargs): start_time = time.time() saver = WorldSaver() elements = set(element_bodies) elements_from_layers = compute_elements_from_layer(elements, layer_from_n) layers = sorted(elements_from_layers.keys()) print('Layers:', layers) full_plan = [] makespan = 0. removed = set() for layer in reversed(layers): print(SEPARATOR) print('Layer: {}'.format(layer)) saver.restore() remaining = elements_from_layers[layer] printed = elements - remaining - removed draw_model(remaining, node_points, ground_nodes, color=GREEN) draw_model(printed, node_points, ground_nodes, color=RED) problem = get_pddlstream(robots, obstacles, node_points, element_bodies, ground_nodes, layer_from_n, printed=printed, removed=removed, return_home=False, trajectories=trajectories, collisions=collisions, disable=disable, **kwargs) layer_plan, certificate = solve_pddlstream(problem, node_points, element_bodies, max_time=max_time - elapsed_time(start_time)) remove_all_debug() if layer_plan is None: return None if post_process: print(SEPARATOR) # Allows the planner to continue to check collisions problem.init[:] = certificate.all_facts #static_facts = extract_static_facts(layer_plan, ...) #problem.init.extend(('Order',) + pair for pair in compute_total_orders(layer_plan)) for fact in [('print', ), ('move', )]: if fact in problem.init: problem.init.remove(fact) new_layer_plan, _ = solve_pddlstream(problem, node_points, element_bodies, planner=POSTPROCESS_PLANNER, max_time=max_time - elapsed_time(start_time)) if (new_layer_plan is not None) and (compute_duration(new_layer_plan) < compute_duration(layer_plan)): layer_plan = new_layer_plan user_input('{:.3f}->{:.3f}'.format( compute_duration(layer_plan), compute_duration(new_layer_plan))) # TODO: replan in a cost sensitive way layer_plan = apply_start(layer_plan, makespan) duration = compute_duration(layer_plan) makespan += duration print( '\nLength: {} | Start: {:.3f} | End: {:.3f} | Duration: {:.3f} | Makespan: {:.3f}' .format(len(layer_plan), compute_start(layer_plan), compute_end(layer_plan), duration, makespan)) full_plan.extend(layer_plan) removed.update(remaining) print(SEPARATOR) print_plan(full_plan) return full_plan, None
def main(): parser = argparse.ArgumentParser() parser.add_argument('paths', nargs='*', help='Paths to the data.') #parser.add_argument('-a', '--active', type=int, default=0, # None # help='The number of active samples to collect') parser.add_argument('-l', '--learner', default=None, help='Path to the learner that should be used') parser.add_argument('-n', '--num_trials', type=int, default=100, help='The number of samples to collect') parser.add_argument('-s', '--save', action='store_true', help='Whether to save the learners') parser.add_argument('-r', '--num_rounds', type=int, default=1, help='The number of rounds to collect') #parser.add_argument('-t', '--num_train', type=int, default=None, # help='The size of the training set') args = parser.parse_args() # TODO: be careful that paging isn't altering the data # TODO: don't penalize if the learner identifies that it can't make a good prediction # TODO: use a different set of randomized parameters for train and test include_none = False serial = is_darwin() #training_sizes = inclusive_range(50, 500, 25) #training_sizes = inclusive_range(25, 100, 5) #training_sizes = inclusive_range(25, 100, 5) training_sizes = inclusive_range(10, 50, 5) #training_sizes = inclusive_range(100, 1000, 100) #training_sizes = [20] #training_sizes = [1500] #kernels = ['RBF', 'Matern52', 'MLP'] kernels = ['MLP'] #hyperparameters = [None] #hyperparameters = [True] hyperparameters = [True, None] # None, query_type = BEST # BEST | CONFIDENT | REJECTION | ACTIVE # type of query used to evaluate the learner is_adaptive = False max_test = 50 # #alphas = np.linspace(0.0, 0.9, num=5, endpoint=True) alphas = [0.0, .8, .9, .99] #alphas = [None] # Use the default (i.e. GP parameters) use_vars = [True] binary = False split = UNIFORM # BALANCED # Omitting failed labels is okay because they will never be executed algorithms = [] #algorithms += [(Algorithm(BATCH_GP, kernel=kernel, hyperparameters=hype, use_var=use_var), [num_train]) # for num_train, kernel, hype, use_var in product(training_sizes, kernels, hyperparameters, use_vars)] algorithms += [(Algorithm(STRADDLE_GP, kernel, hype, use_var), training_sizes) for kernel, hype, use_var in product(kernels, hyperparameters, use_vars)] #algorithms += [(Algorithm(rf_model, p_explore=None, use_var=use_var), [num_train]) # for rf_model, num_train, use_var in product(RF_MODELS, training_sizes, use_vars)] #algorithms += [(Algorithm(nn_model, p_explore=None), [num_train]) # for nn_model, num_train in product(NN_MODELS, training_sizes)] #algorithms += [(Algorithm(RANDOM), None), (Algorithm(DESIGNED), None)] print('Algorithms:', algorithms) print('Split:', split) trials_per_round = sum(1 if train_sizes is None else (train_sizes[-1] - train_sizes[0] + len(train_sizes)) for _, train_sizes in algorithms) num_experiments = args.num_rounds*trials_per_round date_name = datetime.datetime.now().strftime(DATE_FORMAT) size_str = '[{},{}]'.format(training_sizes[0], training_sizes[-1]) #size_str = '-'.join(map(str, training_sizes)) experiments_name = '{}_r={}_t={}_n={}'.format(date_name, args.num_rounds, size_str, args.num_trials) #'19-08-09_21-44-58_r=5_t=[10,150]_n=1'# #experiments_name = 't={}'.format(args.num_rounds) # TODO: could include OS and username if desired domain = load_data(args.paths) print() print(domain) X, Y, W = domain.get_data(include_none=include_none) print('Total number of examples:', len(X)) if binary: # NN can fit perfectly when binary # Binary seems to be outperforming w/o Y = threshold_scores(Y) max_train = len(X) - max_test #min(max([0] + [active_sizes[0] for _, active_sizes in algorithms # if active_sizes is not None]), len(X)) #parameters = { # 'include None': include_none, # 'binary': binary, # 'split': split, #} print('Name:', experiments_name) print('Experiments:', num_experiments) print('Max train:', max_train) print('Include None:', include_none) print('Examples: n={}, d={}'.format(*X.shape)) print('Binary:', binary) print('Estimated hours:', num_experiments * SEC_PER_EXPERIMENT / HOURS_TO_SECS) user_input('Begin?') # TODO: residual learning for sim to real transfer # TODO: can always be conservative and add sim negative examples # TODO: combine all data to write in one folder data_dir = os.path.join(DATA_DIRECTORY, domain.name) # EXPERIMENT_DIRECTORY experiments_dir = os.path.join(data_dir, experiments_name) mkdir(experiments_dir) start_time = time.time() experiments = [] for round_idx in range(args.num_rounds): round_dir = os.path.join(data_dir, experiments_name, str(round_idx)) mkdir(round_dir) seed = hash(time.time()) train_test_file = os.path.join(round_dir, 'data.pk3') if not os.path.exists(train_test_file): X_train, Y_train, X_test, Y_test = split_data(X, Y, split, max_train) X_test, Y_test = X_test[:max_test], Y_test[:max_test] write_pickle(train_test_file, (X_train, Y_train, X_test, Y_test)) else: X_train, Y_train, X_test, Y_test = read_pickle(train_test_file) print('Train examples:', X_train.shape) print('Test examples:', X_test.shape) # TODO: need to be super careful when running with multiple contexts for algorithm, active_sizes in algorithms: # active_sizes = [first #trainingdata selected from X_train, #active exploration + #trainingdata] print(SEPARATOR) print('Round: {} | {} | Seed: {} | Sizes: {}'.format(round_idx, algorithm, seed, active_sizes)) # TODO: allow keyboard interrupt if active_sizes is None: learner = algorithm.name active_size = None train_confusion = None experiments.append(evaluate_learner(domain, seed, train_confusion, X_test, Y_test, algorithm, learner, active_size, args.num_trials, alphas, serial)) else: # [10 20 25] take first 10 samples from X_train to train the model, 10 samples chosen actively # sequentially + evaluate model, 5 samples chosen actively sequentially + evaluate model # Could always keep around all the examples and retrain # TODO: segfaults when this runs in parallel # TODO: may be able to retrain in parallel if I set OPENBLAS_NUM_THREADS learner_prior_nx = 0 ''' if algorithm.hyperparameters: if domain.skill == 'pour': learner_file = '/Users/ziw/ltamp_pr2/data/pour_19-06-13_00-59-21/19-08-09_19-30-01_r=10_t=[50,400]_n=1/{}/gp_active_mlp_true_true.pk3'.format( round_idx) elif domain.skill == 'scoop': learner_file = '/Users/ziw/ltamp_pr2/data/scoop_19-06-10_20-16-59_top-diameter/19-08-09_19-34-56_r=10_t=[50,400]_n=1/{}/gp_active_mlp_true_true.pk3'.format( round_idx) learner = read_pickle(learner_file) learner_prior_nx = learner.nx learner.retrain(newx=X_train[:active_sizes[0]], newy=Y_train[:active_sizes[0], None]) else: ''' learner, train_confusion = create_learner(domain, X_train, Y_train, split, algorithm, num_train=active_sizes[0], query_type=query_type, is_adaptive=is_adaptive) if algorithm.name == STRADDLE_GP: X_select, Y_select = X_train[active_sizes[0]:], Y_train[active_sizes[0]:] for active_size in active_sizes: num_active = active_size - learner.nx + learner_prior_nx# learner.nx is len(learner.xx) print('\nRound: {} | {} | Seed: {} | Size: {} | Active: {}'.format( round_idx, algorithm, seed, active_size, num_active)) if algorithm.name == STRADDLE_GP: X_select, Y_select = active_learning_discrete(learner, num_active, X_select, Y_select) #if args.save: save_learner(round_dir, learner) experiments.append(evaluate_learner(domain, seed, None, X_test, Y_test, algorithm, learner, active_size, args.num_trials, alphas, serial)) save_experiments(experiments_dir, experiments) print(SEPARATOR) if experiments: save_experiments(experiments_dir, experiments) plot_experiments(domain, experiments_name, experiments_dir, experiments, include_none=False) #include_none=include_none) print('Experiments:', experiments_dir) print('Total experiments:', len(experiments)) print('Total hours:', elapsed_time(start_time) / HOURS_TO_SECS)
def main(): connect(use_gui=True) add_data_path() set_camera(0, -30, 1) plane = load_pybullet('plane.urdf', fixed_base=True) #plane = load_model('plane.urdf') cup = load_model('models/cup.urdf', fixed_base=True) #set_point(cup, Point(z=stable_z(cup, plane))) set_point(cup, Point(z=.2)) set_color(cup, (1, 0, 0, .4)) #wait_for_interrupt() num_droplets = 100 #radius = 0.025 #radius = 0.005 radius = 0.0025 # TODO: more efficient ways to make all of these droplets = [create_sphere(radius, mass=0.01) for _ in range(num_droplets)] # kg cup_thickness = 0.001 lower, upper = get_lower_upper(cup) print(lower, upper) buffer = cup_thickness + radius lower = np.array(lower) + buffer * np.ones(len(lower)) upper = np.array(upper) - buffer * np.ones(len(upper)) limits = zip(lower, upper) x_range, y_range = limits[:2] z = upper[2] + 0.1 #x_range = [-1, 1] #y_range = [-1, 1] #z = 1 for droplet in droplets: x = np.random.uniform(*x_range) y = np.random.uniform(*y_range) set_point(droplet, Point(x, y, z)) for i, droplet in enumerate(droplets): x, y = np.random.normal(0, 1e-3, 2) set_point(droplet, Point(x, y, z + i * (2 * radius + 1e-3))) #dump_world() wait_for_interrupt() #user_input('Start?') #dt = 1. / 240 dt = 0.01 #dt = 0 print('dt:', dt) enable_gravity() simulate_for_duration(5.0, dt=dt) # enable_real_time() # try: # while True: # enable_gravity() # enable_real_time requires a command # #time.sleep(dt) # except KeyboardInterrupt: # pass # print() #time.sleep(1.0) #wait_for_interrupt() user_input('Finish?') disconnect()
def train_parallel(args, n=1): from extrusion.run import plan_extrusion assert SKIP_PERCENTAGE == 0 initial_time = time.time() problems = sorted(set(enumerate_problems()) - set(EXCLUDE)) #problems = ['four-frame'] #problems = ['simple_frame', 'topopt-101_tiny', 'topopt-100_S1_03-14-2019_w_layer'] algorithms = list(ALGORITHMS) if args.disable: for algorithm in LOOKAHEAD_ALGORITHMS: if algorithm in algorithms: algorithms.remove(algorithm) #algorithms = ['regression'] heuristics = HEURISTICS #heuristics = DISTANCE_HEURISTICS + COST_HEURISTICS seeds = list(range(args.num)) if n is None: n = len(seeds) groups = list(chunks(seeds, n=n)) print('Chunks: {}'.format(len(groups))) print('Problems ({}): {}'.format(len(problems), problems)) #problems = [path for path in problems if 'simple_frame' in path] print('Algorithms ({}): {}'.format(len(algorithms), algorithms)) print('Heuristics ({}): {}'.format(len(heuristics), heuristics)) jobs = [[ Configuration(seed, problem, algorithm, heuristic, args.max_time, args.cfree, args.disable, args.stiffness, args.motions, args.ee_only) for seed, algorithm, heuristic in product( group, algorithms, heuristics) ] for problem, group in product(problems, groups)] # TODO: separate out the algorithms again # TODO: print the size per job print('Jobs: {}'.format(len(jobs))) serial = is_darwin() available_cores = cpu_count() num_cores = max(1, min(1 if serial else available_cores - 4, len(jobs))) print('Max Cores:', available_cores) print('Serial:', serial) print('Using Cores:', num_cores) date = datetime.datetime.now().strftime(DATE_FORMAT) filename = '{}.pk{}'.format(date, get_python_version()) path = os.path.join(EXPERIMENTS_DIR, filename) print('Data path:', path) user_input('Begin?') start_time = time.time() timeouts = 0 pool = Pool(processes=num_cores) # , initializer=mute) generator = pool.imap_unordered(plan_extrusion, jobs, chunksize=1) results = [] while True: # TODO: randomly sort instead last_time = time.time() try: for config, data in generator.next(): # timeout=2 * args.max_time) results.append((config, data)) print('{}/{} completed | {:.3f} seconds | timeouts: {} | {}'. format(len(results), len(jobs), elapsed_time(start_time), timeouts, datetime.datetime.now().strftime(DATE_FORMAT))) print(config, data) if results: write_pickle(path, results) print('Saved', path) except StopIteration: break # except TimeoutError: # # TODO: record this as a failure? Nothing is saved though... # timeouts += 1 # #traceback.print_exc() # print('Error! Timed out after {:.3f} seconds'.format(elapsed_time(last_time))) # break # This kills all jobs # #continue # This repeats jobs until success print('Total time:', elapsed_time(initial_time)) return results
def main(): parser = create_parser() args = parser.parse_args() print(args) # https://stackoverflow.com/questions/15314189/python-multiprocessing-pool-hangs-at-join # https://stackoverflow.com/questions/39884898/large-amount-of-multiprocessing-process-causing-deadlock # TODO: alternatively don't destroy the world num_cores = max(1, cpu_count() - SPARE_CORES) json_path = os.path.abspath( os.path.join(EXPERIMENTS_DIRECTORY, '{}.json'.format(get_date()))) #memory_per_core = float(MAX_RAM) / num_cores # gigabytes #set_soft_limit(resource.RLIMIT_AS, int(BYTES_PER_GIGABYTE * memory_per_core)) # bytes #set_soft_limit(resource.RLIMIT_CPU, 2*MAX_TIME) # seconds # RLIMIT_MEMLOCK, RLIMIT_STACK, RLIMIT_DATA print('Results:', json_path) print('Num Cores:', num_cores) #print('Memory per Core: {:.2f}'.format(memory_per_core)) print('Tasks: {} | {}'.format(len(TASK_NAMES), TASK_NAMES)) print('Policies: {} | {}'.format(len(POLICIES), POLICIES)) print('Num Trials:', N_TRIALS) num_experiments = len(TASK_NAMES) * len(POLICIES) * N_TRIALS print('Num Experiments:', num_experiments) max_parallel = math.ceil(float(num_experiments) / num_cores) print('Estimated duration: {:.2f} hours'.format( MEAN_TIME_PER_TRIAL * max_parallel / HOURS_TO_SECS)) user_input('Begin?') print(SEPARATOR) print('Creating problems') start_time = time.time() problems = create_problems(args) experiments = [ { 'problem': copy.deepcopy(problem), 'policy': policy } #, 'args': args} for problem in problems for policy in POLICIES ] print('Created {} problems and {} experiments in {:.3f} seconds'.format( len(problems), len(experiments), elapsed_time(start_time))) print(SEPARATOR) ensure_dir(EXPERIMENTS_DIRECTORY) safe_rm_dir(TEMP_DIRECTORY) ensure_dir(TEMP_DIRECTORY) start_time = time.time() results = [] try: for result in map_parallel(run_experiment, experiments, num_cores=num_cores): results.append(result) print('{}\nExperiments: {} / {} | Time: {:.3f}'.format( SEPARATOR, len(results), len(experiments), elapsed_time(start_time))) print('Experiment:', str_from_object(result['experiment'])) print('Outcome:', str_from_object(result['outcome'])) write_json(json_path, results) #except BaseException as e: # traceback.print_exc() # e finally: if results: write_json(json_path, results) print(SEPARATOR) print('Saved:', json_path) print('Results:', len(results)) print('Duration / experiment: {:.3f}'.format( num_cores * elapsed_time(start_time) / len(experiments))) print('Duration: {:.2f} hours'.format( elapsed_time(start_time) / HOURS_TO_SECS)) safe_rm_dir(TEMP_DIRECTORY) # TODO: dump results automatically? return results
def get_input(message, options): full_message = '{} [{}]: '.format(message, ','.join(options)) response = user_input(full_message) while response not in options: response = user_input(full_message) return response
def main(): assert (get_python_version() == 2) # ariadne has ROS with python2 parser = argparse.ArgumentParser() parser.add_argument('-p', '--problem', default='test_pick', help='The name of the problem to solve.') parser.add_argument( '-e', '--execute', action='store_true', help='When enabled, executes the plan using physics simulation.') parser.add_argument( '-c', '--cfree', action='store_true', help='When enabled, disables collision checking (for debugging).') parser.add_argument( '-l', '--learning', action='store_true', help='When enabled, uses learned generators when applicable.') parser.add_argument( '-v', '--visualize_planning', action='store_true', help= 'When enabled, visualizes planning rather than the world (for debugging).' ) args = parser.parse_args() task_fn = get_task_fn(PROBLEMS, args.problem) task = task_fn() print('Task:', task.arms) ros_world = ROSWorld(sim_only=False, visualize=not args.visualize_planning) reset_robot(task, ros_world) ros_world.perception.wait_for_update() if task.use_kitchen: add_table_surfaces(ros_world) ros_world.sync_controllers() # AKA update sim robot joint positions ros_world.perception.update_simulation() add_holding(task, ros_world) print('Surfaces:', ros_world.perception.get_surfaces()) print('Items:', ros_world.perception.get_items()) draw_names(ros_world) draw_forward_reachability(ros_world, task.arms) planning_world = PlanningWorld(task, visualize=args.visualize_planning) planning_world.load(ros_world) print(planning_world) plan = plan_actions(planning_world, collisions=not args.cfree) if plan is None: print('Failed to find a plan') user_input('Finish?') return if not args.execute and not review_plan(task, ros_world, plan): return if args.cfree: print('Aborting execution') return #sparsify_plan(plan) simulator = ros_world.alternate_controller if args.execute else None if simulator is not None: simulator.set_gravity() finished = execute_plan(ros_world, plan, joint_speed=0.25) print('Finished:', finished)
def main(): parser = argparse.ArgumentParser() parser.add_argument('paths', nargs='*', help='Paths to the data.') #parser.add_argument('-a', '--active', type=int, default=0, # None # help='The number of active samples to collect') parser.add_argument( '-d', '--deterministic', action='store_true', help='Whether to deterministically create training splits') parser.add_argument('-n', '--num_trials', type=int, default=-1, help='The number of samples to collect') parser.add_argument('-s', '--save', action='store_true', help='Whether to save the learners') parser.add_argument('-r', '--num_rounds', type=int, default=1, help='The number of rounds to collect') parser.add_argument('-t', '--test', action='store_true', help='Whether to save the data') parser.add_argument('-v', '--visualize', action='store_true', help='When enabled, visualizes execution.') args = parser.parse_args() # TODO: be careful that paging isn't altering the data # TODO: use a different set of randomized parameters for train and test serial = is_darwin() visualize = serial and args.visualize assert implies(visualize, serial) num_trials = get_max_cores( serial) if args.num_trials < 0 else args.num_trials ################################################## #train_sizes = inclusive_range(50, 200, 10) # Best #train_sizes = inclusive_range(50, 400, 10) # F1 #train_sizes = inclusive_range(25, 400, 25) #train_sizes = inclusive_range(50, 100, 5) # Real #train_sizes = inclusive_range(100, 200, 5) #train_sizes = inclusive_range(10, 250, 5) #train_sizes = inclusive_range(35, 70, 5) #train_sizes = inclusive_range(5, 50, 5) #train_sizes = inclusive_range(40, 80, 5) #train_sizes = inclusive_range(100, 1000, 100) #train_sizes = [50] #train_sizes = [250] train_sizes = [1000] #train_sizes = [327] # train + test #train_sizes = inclusive_range(5, 150, 25) #train_sizes = [100] #kernels = ['RBF', 'Matern52', 'MLP'] kernels = ['MLP'] hyperparams = [None] #hyperparams = [True] #hyperparams = [None, True] query_type = BEST # BEST | CONFIDENT | REJECTION | ACTIVE # type of query used to evaluate the learner include_none = False binary = False # 0 => no transfer # 1 => mean transfer # 2 => kernel transfer # 3 => both transfer transfer_weights = [None] #transfer_weights = list(range(4)) #transfer_weights = [0, 1] #transfer_weights = [3] #transfer_weights = np.around(np.linspace(0.0, 1.0, num=1+5, endpoint=True), decimals=3) # max 10 colors #transfer_weights = list(range(1, 1+3)) #split = UNIFORM # BALANCED #print('Split:', split) #parameters = { # 'include None': include_none, # 'binary': binary, # 'split': split, #} # Omitting failed labels is okay because they will never be executed algorithms = [] #algorithms += [(Algorithm(nn_model, label='NN'), [num]) # for nn_model, num in product(NN_MODELS, train_sizes)] #algorithms += [(Algorithm(RANDOM), None), (Algorithm(DESIGNED), None)] #algorithms += [(Algorithm(RF_CLASSIFIER, variance=False, transfer_weight=tw, label='RF'), [num]) # for num, tw in product(train_sizes, [None])] # transfer_weights #algorithms += [(Algorithm(RF_REGRESSOR, variance=False, transfer_weight=tw, label='RF'), [num]) # for num, tw in product(train_sizes, [None])] # transfer_weights #algorithms += [(Algorithm(BATCH_RF, variance=True, transfer_weight=tw, label='RF'), [num]) # for num, tw in product(train_sizes, [None])] # transfer_weights #algorithms += [(Algorithm(BATCH_MAXVAR_RF, variance=True, transfer_weight=tw), train_sizes) # for tw in product(use_vars, [None])] # transfer_weights #algorithms += [(Algorithm(BATCH_STRADDLE_RF, variance=True, transfer_weight=tw), train_sizes) # for tw, in product([None])] # transfer_weights use_vars = [True] # STRADDLE is better than MAXVAR when the learner has a good estimate of uncertainty algorithms += [ (Algorithm(BATCH_GP, kernel, hype, use_var, tw, label='GP'), [num]) # label='GP-{}'.format(kernel) for num, kernel, hype, use_var, tw in product( train_sizes, kernels, hyperparams, use_vars, transfer_weights) ] #algorithms += [(Algorithm(BATCH_MAXVAR_GP, kernel, hype, True, tw, label='GP-Var'), train_sizes) # for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)] #algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hype, True, tw, label='GP-LSE'), train_sizes) # for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)] # default active #algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hype, True, tw, label='GP-LSE2'), train_sizes) # for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)] # active control only # algorithms += [(Algorithm(MAXVAR_GP, kernel, hype, use_var), train_sizes) # for kernel, hype, use_var in product(kernels, hyperparams, use_vars)] #algorithms += [(Algorithm(STRADDLE_GP, kernel, hype, use_var, tw), train_sizes) # for kernel, hype, use_var, tw in product(kernels, hyperparams, use_vars, transfer_weights)] #batch_sizes = inclusive_range(train_sizes[0], 90, 10) #step_size = 10 # TODO: extract from train_sizes #final_size = train_sizes[-1] # Previously didn't have use_var=True # algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hyperparameters=batch_size, variance=True, transfer_weight=tw), # inclusive_range(batch_size, final_size, step_size)) # for kernel, tw, batch_size in product(kernels, transfer_weights, batch_sizes)] # algorithms += [(Algorithm(BATCH_STRADDLE_RF, hyperparameters=batch_size, variance=True, transfer_weight=tw), # inclusive_range(batch_size, final_size, step_size)) # for tw, batch_size in product(transfer_weights, batch_sizes)] print('Algorithms:', algorithms) ################################################## real_world = not args.paths transfer_domain = load_data(TRANSFER_DATASETS, verbose=False) transfer_algorithm = None if real_world and transfer_weights != [None]: #assert transfer_weights[0] is not None transfer_data = transfer_domain.create_dataset( include_none=include_none, binary=binary) transfer_algorithm = Algorithm(BATCH_GP, kernel=kernels[0], variance=use_vars[0]) validity_learner = None #validity_learner = create_validity_classifier(transfer_domain) ################################################## train_paths = args.paths if real_world: train_paths = SCOOP_TRAIN_DATASETS # TRAIN_DATASETS #train_paths = TRANSFER_DATASETS #train_paths = TRAIN_DATASETS + TRANSFER_DATASETS # Train before transfer #scale_paths = TRAIN_DATASETS + TEST_DATASETS scale_paths = None print(SEPARATOR) print('Train paths:', train_paths) domain = load_data(train_paths) print() print(domain) all_data = domain.create_dataset(include_none=include_none, binary=binary, scale_paths=scale_paths) #all_data.results = all_data.results[:1000] num_failed = 0 #num_failed = 100 failed_domain = transfer_domain if real_world else domain failed_results = randomize( result for result in failed_domain.results if not result.get('success', False))[:num_failed] #failed_data = Dataset(domain, failed_results, **all_data.kwargs) test_paths = SCOOP_TEST_DATASETS # TEST_DATASETS | SCOOP_TEST_DATASETS #test_paths = None if real_world and not (set(train_paths) & set(test_paths)): #assert not set(train_paths) & set(test_paths) #max_test = 0 test_data = load_data(test_paths).create_dataset( include_none=False, binary=binary, scale_paths=scale_paths) else: #assert scale_paths is None # TODO: max_train will be too small otherwise test_paths = test_data = None print(SEPARATOR) print('Test paths:', test_paths) all_active_data = None #if real_world: # all_active_data = load_data(ACTIVE_DATASETS).create_dataset(include_none=True, binary=binary, scale_paths=scale_paths) # TODO: could include OS and username if desired date_name = datetime.datetime.now().strftime(DATE_FORMAT) size_str = '[{},{}]'.format(train_sizes[0], train_sizes[-1]) #size_str = '-'.join(map(str, train_sizes)) experiments_name = '{}_r={}_t={}_n={}'.format(date_name, args.num_rounds, size_str, num_trials) trials_per_round = sum( 1 if train_sizes is None else (train_sizes[-1] - train_sizes[0] + len(train_sizes)) for _, train_sizes in algorithms) num_experiments = args.num_rounds * trials_per_round max_train = min( max([0] + [ active_sizes[0] for _, active_sizes in algorithms if active_sizes is not None ]), len(all_data)) max_test = min(len(all_data) - max_train, 1000) ################################################## # #features = ['bowl_height'] # features = ['spoon_height'] # #features = ['bowl_height', 'spoon_height'] # X, Y, _ = all_data.get_data() # #indices = [domain.inputs.index(feature) for feature in features] # #X = X[:,indices] # X = [[result[FEATURE][name] for name in features] for result in all_data.results] # from sklearn.linear_model import LinearRegression # model = LinearRegression(fit_intercept=True, normalize=False) # model.fit(X, Y) # #print(model.get_params()) # print(model.coef_.tolist(), model.intercept_) # print(model.score(X, Y)) #data_dir = os.path.join(DATA_DIRECTORY, domain.name) # EXPERIMENT_DIRECTORY data_dir = os.path.abspath(os.path.join(domain.name, os.path.pardir)) experiments_dir, data_path = None, None if not args.test or not serial: experiments_dir = os.path.join(data_dir, experiments_name) data_path = os.path.join( experiments_dir, 'experiments.pk{}'.format(get_python_version())) ################################################## print(SEPARATOR) print('Name:', experiments_name) print('Experiments:', num_experiments) print('Experiment dir:', experiments_dir) print('Data path:', data_path) print('Examples:', len(all_data)) print('Valid:', sum(result.get('valid', True) for result in all_data.results)) print('Success:', sum(result.get('success', False) for result in all_data.results)) print( 'Scored:', sum( result.get('score', None) is not None for result in all_data.results)) print('Max train:', max_train) print('Max test:', max_test) print('Include None:', include_none) print('Examples: n={}, d={}'.format(len(all_data), domain.dx)) print('Binary:', binary) print('Serial:', serial) print('Estimated hours: {:.3f}'.format(num_experiments * SEC_PER_EXPERIMENT / HOURS_TO_SECS)) user_input('Begin?') ################################################## experiments = [] if experiments_dir is not None: mkdir(experiments_dir) # if os.path.exists(data_path): # experiments.extend(read_pickle(data_path)) # TODO: embed in a KeyboardInterrupt to allow early termination start_time = time.time() for round_idx in range(args.num_rounds): seed = round_idx if args.deterministic else hash( time.time()) # vs just time.time()? random.seed(seed) all_data.shuffle() if test_paths is None: # cannot use test_data #test_data, train_data = split_data(all_data, max_test) train_data = test_data = all_data # Training performance else: train_data = all_data transfer_learner = None if transfer_algorithm is not None: round_data, _ = transfer_data.partition(index=1000) transfer_learner, _ = create_learner(transfer_domain, round_data, transfer_algorithm, verbose=True) transfer_learner.retrain() print(SEPARATOR) print('Round {} | Train examples: {} | Test examples: {}'.format( round_idx, len(train_data), len(test_data))) for algorithm, active_sizes in algorithms: # active_sizes = [first #trainingdata selected from X_train, #active exploration + #trainingdata] print(SEPARATOR) print('Round: {} | {} | Seed: {} | Sizes: {}'.format( round_idx, algorithm, seed, active_sizes)) # TODO: allow keyboard interrupt if active_sizes is None: learner = algorithm.name active_size = train_confusion = None experiments.append( evaluate_learner(domain, seed, train_confusion, test_data, algorithm, learner, active_size, num_trials, serial, args.visualize)) continue # [10 20 25] take first 10 samples from X_train to train the model, 10 samples chosen actively # sequentially + evaluate model, 5 samples chosen actively sequentially + evaluate model # Could always keep around all the examples and retrain # TODO: segfaults when this runs in parallel # TODO: may be able to retrain in parallel if I set OPENBLAS_NUM_THREADS num_batch = active_sizes[0] batch_data, active_data = train_data.partition(num_batch) if all_active_data is not None: active_data = all_active_data.clone() #batch_data.results.extend(failed_results) learner, train_confusion = create_learner( domain, batch_data, algorithm, # alphas, query_type=query_type, verbose=True) learner.validity_learner = validity_learner if transfer_learner is not None: learner.sim_model = transfer_learner.model learner.retrain() for active_size in active_sizes: num_active = active_size - (learner.nx - len(failed_results)) print('\nRound: {} | {} | Seed: {} | Size: {} | Active: {}'. format(round_idx, algorithm, seed, active_size, num_active)) if algorithm.name in CONTINUOUS_ACTIVE_GP: active_learning(learner, num_active, visualize=visualize) #active_learning(learner, num_active, discrete_feature=True, random_feature=False) #active_learning_discrete(learner, active_data, num_active, random_feature=False) elif algorithm.name in BATCH_ACTIVE: active_learning_discrete(learner, active_data, num_active) #active_learning(learner, num_active, discrete_feature=True, random_feature=True) #active_learning_discrete(learner, active_data, num_active, random_feature=True) #if round_dir is not None: # save_learner(round_dir, learner) if args.save: learner.save(data_dir) experiments.append( evaluate_learner(domain, seed, train_confusion, test_data, algorithm, learner, active_size, num_trials, serial, args.visualize)) save_experiments(data_path, experiments) print(SEPARATOR) if experiments: save_experiments(data_path, experiments) plot_experiments(domain, experiments_name, experiments_dir, experiments, include_none=False) print('Experiments: {}'.format(experiments_dir)) print('Total experiments: {}'.format(len(experiments))) print('Total hours: {:.3f}'.format( elapsed_time(start_time) / HOURS_TO_SECS))
def main(execute='execute'): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') #parser.add_argument('-display', action='store_true', help='enable viewer.') args = parser.parse_args() #display = args.display display = True connect(use_gui=args.viewer) robot, block = load_world() #robot2 = clone_body(robot) #block2 = clone_body(block) #dump_world() saved_world = WorldSaver() dump_world() ss_problem = ss_from_problem(robot, movable=[block], teleport=False, movable_collisions=True) #ss_problem = ss_problem.debug_problem() #print(ss_problem) t0 = time.time() plan, evaluations = dual_focused(ss_problem, verbose=True) # plan, evaluations = incremental(ss_problem, verbose=True) print_plan(plan, evaluations) print(time.time() - t0) if (not display) or (plan is None): disconnect() return paths = [] for action, params in plan: if action.name == 'place': paths += params[-1].reverse().body_paths elif action.name in ['move_free', 'move_holding', 'pick']: paths += params[-1].body_paths print(paths) command = Command(paths) if not args.viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() saved_world.restore() user_input('Execute?') if execute == 'control': command.control() elif execute == 'execute': command.refine(num_steps=10).execute(time_step=0.001) elif execute == 'step': command.step() else: raise ValueError(execute) #dt = 1. / 240 # Bullet default #p.setTimeStep(dt) wait_for_interrupt() disconnect()