def custom_bb_slider(): """ Generate a custom BusyBox environment """ from gen.generator_busybox import Slider, BusyBox bb_width = 0.8 bb_height = 0.4 x_offset = 0.0 z_offset = 0.0 range = 0.3 angle = np.pi / 4 axis = (np.cos(angle), np.sin(angle)) color = [1, 0, 0] slider = Slider(x_offset, z_offset, range, axis, color) return BusyBox.get_busybox(bb_width, bb_height, [slider])
def main(args): busybox_data = get_bb_dataset(args.bb_fname, args.N, args.mech_types, 1, args.urdf_num) all_steps = [] for ix, bb_results in enumerate(busybox_data): if args.type == 'gpucb': single_dataset, _, steps = create_single_bb_gpucb_dataset( bb_results[0], '', args.plot, args, ix, success_regret=SUCCESS_REGRET) all_steps.append(steps) print('steps', steps) elif args.type == 'random': bb = BusyBox.bb_from_result(bb_results[0]) image_data, gripper = setup_env(bb, args.viz, args.debug) regret = float("inf") steps = 0 while regret > SUCCESS_REGRET: mech = bb._mechanisms[0] # generate either a random or model-based policy and goal configuration policy = policies.generate_policy(mech) pose_handle_world_init = mech.get_handle_pose() # calculate trajectory pose_handle_base_world = mech.get_pose_handle_base_world() traj = policy.generate_trajectory(pose_handle_base_world, args.debug, color=[0, 0, 1]) # execute trajectory cumu_motion, net_motion, pose_handle_world_final = \ gripper.execute_trajectory(traj, mech, policy.type, args.debug) # calc regret max_dist = mech.get_max_net_motion() regret = (max_dist - net_motion) / max_dist steps += 1 # reset gripper.reset(mech) p.disconnect() all_steps.append(steps) print('steps', steps) # Save the dataset. util.write_to_file(args.fname, all_steps)
def custom_bb_door(): """ Generate a custom BusyBox environment """ from gen.generator_busybox import Door, BusyBox bb_width = 0.8 bb_height = 0.4 door_offset = (0.0, 0.0) door_size = (0.15, 0.15) handle_offset_z = 0.0 flipped = True door = Door(door_offset, door_size, handle_offset_z, flipped, color=[1, 0, 0]) return BusyBox.get_busybox(bb_width, bb_height, [door])
def replay_result(result): from gen.generator_busybox import BusyBox from utils.setup_pybullet import setup_env from actions.policies import get_policy_from_tuple bb = BusyBox.bb_from_result(result) image_data, gripper = setup_env(bb, True, True) mech = bb._mechanisms[0] policy = get_policy_from_tuple(result.policy_params) pose_handle_base_world = mech.get_pose_handle_base_world() traj = policy.generate_trajectory(pose_handle_base_world, True) cumu_motion, net_motion, pose_handle_world_final = gripper.execute_trajectory( traj, mech, policy.type, True) import pdb pdb.set_trace() p.disconnect()
def plot_test_set(): # plot test data angles and positions for (i, point) in enumerate(test_data[:10]): bb = BusyBox.bb_from_result(point) if p.getConnectionInfo()['isConnected']: p.disconnect() setup_pybullet.setup_env(bb, False, False) mech = bb._mechanisms[0] true_policy = policies.generate_policy(mech, False) pos = (true_policy.rigid_position[0], true_policy.rigid_position[2]) pos_ax.plot(*pos, 'm.') pos_ax.annotate(i, pos) if i == len(test_data): angle_ax.plot(true_policy.pitch, 2.0, 'k.', label='test data') else: angle_ax.plot(true_policy.pitch, 2.0, 'k.') angle_ax.annotate(i, (true_policy.pitch, 2.0))
def get_true_ys(X_pred, mech, policy_params): Y_pred = np.zeros((X_pred.shape[0])) viz = False debug = False for i, x in enumerate(X_pred): policy = policies.get_policy_from_x(mech, x, policy_params) width, height = 0.6, 0.6 bb = BusyBox.get_busybox(width, height, [mech]) _, gripper = setup_env(bb, viz, debug) # calculate trajectory pose_handle_base_world = mech.get_pose_handle_base_world() traj = policy.generate_trajectory(pose_handle_base_world, debug) # execute trajectory _, net_motion, _ = gripper.execute_trajectory(traj, mech, policy.type, False) Y_pred[i] = net_motion gripper.reset(mech) return Y_pred
def generate_dataset(args, git_hash): bb_dataset = get_bb_dataset(args.bb_fname, args.n_bbs, args.mech_types, args.max_mech, args.urdf_num) if args.n_samples == 0: return bb_dataset results = [] for (i, bb_results) in enumerate(bb_dataset): bb = BusyBox.bb_from_result(bb_results[0]) image_data, gripper = setup_env(bb, args.viz, args.debug) bb_results = [] for j in range(args.n_samples): sys.stdout.write("\rProcessing sample %i/%i for busybox %i/%i" % (j+1, args.n_samples, i+1, args.n_bbs)) for mech in bb._mechanisms: # generate either a random or model-based policy and goal configuration policy = policies.generate_policy(mech) pose_handle_world_init = mech.get_handle_pose() # calculate trajectory pose_handle_base_world = mech.get_pose_handle_base_world() traj = policy.generate_trajectory(pose_handle_base_world, args.debug, color=[0, 0, 1]) # execute trajectory cumu_motion, net_motion, pose_handle_world_final = \ gripper.execute_trajectory(traj, mech, policy.type, args.debug) # save result data policy_params = policy.get_policy_tuple() mechanism_params = mech.get_mechanism_tuple() result = util.Result(policy_params, mechanism_params, net_motion, \ cumu_motion, pose_handle_world_init, pose_handle_world_final, \ image_data, git_hash) bb_results.append(result) gripper.reset(mech) results.append(bb_results) p.disconnect() print() return results
def get_bb_dataset(bb_fname, n_bbs, mech_types, max_mech, urdf_num): # Create a dataset of busyboxes. if bb_fname == '' or bb_fname is None: print('Creating Busyboxes.') mech_classes = [] for mech_type in mech_types: if mech_type == 'slider': mech_classes.append(Slider) if mech_type == 'door': mech_classes.append(Door) bb_dataset = [] for _ in range(n_bbs): # TODO: i think there is a bug here... bb = BusyBox.generate_random_busybox(max_mech=max_mech, mech_types=mech_classes, urdf_tag=urdf_num) mechanism_params = bb._mechanisms[0].get_mechanism_tuple() image_data, gripper = setup_env(bb, False, False) bb_dataset.append([util.Result(None, mechanism_params, None, None, None, None, image_data, None)]) print('BusyBoxes created.') else: # Load in a file with predetermined BusyBoxes. bb_dataset = read_from_file(bb_fname)[:n_bbs] return bb_dataset
def create_single_bb_gpucb_dataset(bb_result, nn_fname, plot, args, bb_i, n_interactions=None, plot_dir_prefix='', ret_regret=False, success_regret=None): use_cuda = False dataset = [] viz = False debug = False # interact with BB bb = BusyBox.bb_from_result(bb_result, urdf_num=args.urdf_num) mech = bb._mechanisms[0] image_data, gripper = setup_env(bb, viz, debug) pose_handle_base_world = mech.get_pose_handle_base_world() sampler = UCB_Interaction(bb, image_data, plot, args, nn_fname=nn_fname) for ix in itertools.count(): gripper.reset(mech) if args.debug: sys.stdout.write('\rProcessing sample %i' % ix) # if we are done sampling n_interactions OR we need to get regret after # each interaction if ((not n_interactions is None) and ix==n_interactions) or \ (not success_regret is None): regret, start_x, stop_x, policy_type = test_model(sampler, args, gripper=gripper) gripper.reset(mech) #print('Current regret', regret) opt_points = (policy_type, [(start_x, 'g'), (stop_x, 'r')]) sample_points = {'Prismatic': [(sample, 'k') for sample in sampler.xs['Prismatic']], 'Revolute': [(sample, 'k') for sample in sampler.xs['Revolute']]} # if done sampling n_interactions if (not n_interactions is None) and ix==n_interactions: if plot: viz_circles(util.GP_PLOT, image_data, mech, BETA, sample_points=sample_points, opt_points=opt_points, gps=sampler.gps, nn=sampler.nn, bb_i=bb_i, plot_dir_prefix=plot_dir_prefix) plt.show() input('Enter to close') plt.close() if ret_regret: return dataset, sampler.gps, regret else: return dataset, sampler.gps # if got successful interaction or timeout elif (not success_regret is None) and \ ((regret < success_regret) or (ix >= 100)): if ix >= 100: print('timeout interactions') elif regret < success_regret: print('succcessful interaction!') if plot: viz_circles(util.GP_PLOT, image_data, mech, BETA, sample_points=sample_points, opt_points=opt_points, gps=sampler.gps, nn=sampler.nn, bb_i=bb_i, plot_dir_prefix=plot_dir_prefix) return dataset, sampler.gps, ix # sample a policy # image_data, gripper = setup_env(bb, False, debug) gripper.reset(mech) x, policy = sampler.sample() # execute traj = policy.generate_trajectory(pose_handle_base_world, debug=debug) c_motion, motion, handle_pose_final = gripper.execute_trajectory(traj, mech, policy.type, False) result = util.Result(policy.get_policy_tuple(), mech.get_mechanism_tuple(), motion, c_motion, handle_pose_final, handle_pose_final, image_data, None) dataset.append(result) # update GP sampler.update(result, x)
def viz_train_test_data(train_data, test_data): import matplotlib.pyplot as plt from gen.generator_busybox import BusyBox n_inter_per_bb = 100 n_bbs = int(len(train_data) / n_inter_per_bb) training_dataset_size = n_bbs #10 plt.ion() angle_fig, angle_ax = plt.subplots() pos_fig, pos_ax = plt.subplots() def plot_test_set(): # plot test data angles and positions for (i, point) in enumerate(test_data[:10]): bb = BusyBox.bb_from_result(point) if p.getConnectionInfo()['isConnected']: p.disconnect() setup_pybullet.setup_env(bb, False, False) mech = bb._mechanisms[0] true_policy = policies.generate_policy(mech, False) pos = (true_policy.rigid_position[0], true_policy.rigid_position[2]) pos_ax.plot(*pos, 'm.') pos_ax.annotate(i, pos) if i == len(test_data): angle_ax.plot(true_policy.pitch, 2.0, 'k.', label='test data') else: angle_ax.plot(true_policy.pitch, 2.0, 'k.') angle_ax.annotate(i, (true_policy.pitch, 2.0)) dataset_sizes = list( range(training_dataset_size, n_bbs + 1, training_dataset_size)) pitches = [] # plot training data angles and positions for n in range(1, n_bbs + 1): point = train_data[(n - 1) * n_inter_per_bb] if p.getConnectionInfo()['isConnected']: p.disconnect() bb = BusyBox.bb_from_result(point) setup_pybullet.setup_env(bb, False, False) mech = bb._mechanisms[0] true_policy = policies.generate_policy(mech, False) pitches += [true_policy.pitch] pos = (true_policy.rigid_position[0], true_policy.rigid_position[2]) pos_ax.plot(*pos, 'c.') if n in dataset_sizes: angle_ax.clear() pos_ax.set_title('Positions of Training and Test Data, n_bbs=' + str(n)) pos_fig.savefig('dataset_imgs/poss_n_bbs' + str(n)) angle_ax.set_title('Historgram of Training Data Angle, n_bbs=' + str(n)) angle_ax.hist(pitches, 30) plot_test_set() #angle_fig.savefig('dataset_imgs/pitches_n_bbs'+str(n)) plt.show() input('enter to close') angle_ax.set_title('Historgram of Training Data Angle, n_bbs=' + str(n)) angle_ax.hist(pitches, 30) plot_test_set() #angle_fig.savefig('dataset_imgs/pitches_n_bbs'+str(n)) plt.show() input('enter to close')