'experiment_dir': EXP_DIR, 'data_files_dir': EXP_DIR + 'data_files/', 'target_filename': EXP_DIR + 'target.npz', 'log_filename': EXP_DIR + 'log.txt', 'conditions': 2, } x0s = [] ee_tgts = [] reset_conditions = [] # Set up each condition. for i in xrange(common['conditions']): ja_x0, ee_pos_x0, ee_rot_x0 = load_pose_from_npz( common['target_filename'], 'trial_arm', str(i), 'initial' ) ja_aux, _, _ = load_pose_from_npz( common['target_filename'], 'auxiliary_arm', str(i), 'initial' ) _, ee_pos_tgt, ee_rot_tgt = load_pose_from_npz( common['target_filename'], 'trial_arm', str(i), 'target' ) x0 = np.zeros(32) x0[:7] = ja_x0 x0[14:(14+9)] = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_x0, ee_rot_x0).T ) ee_tgt = np.ndarray.flatten(
'experiment_dir': EXP_DIR, 'data_files_dir': EXP_DIR + 'data_files/', 'target_filename': EXP_DIR + 'target.npz', 'log_filename': EXP_DIR + 'log.txt', 'conditions': 2, } x0s = [] ee_tgts = [] reset_conditions = [] # Set up each condition. for i in xrange(common['conditions']): ja_x0, ee_pos_x0, ee_rot_x0 = load_pose_from_npz(common['target_filename'], 'trial_arm', str(i), 'initial') ja_aux, _, _ = load_pose_from_npz(common['target_filename'], 'auxiliary_arm', str(i), 'initial') _, ee_pos_tgt, ee_rot_tgt = load_pose_from_npz(common['target_filename'], 'trial_arm', str(i), 'target') x0 = np.zeros(32) x0[:7] = ja_x0 x0[14:(14 + 9)] = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_x0, ee_rot_x0).T) ee_tgt = np.ndarray.flatten( get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T)
def main(): parser = argparse.ArgumentParser( description='Evaluate Policy Sample Data.') parser.add_argument('-lqr', '--lqr_experiment_folder', type=str, help='Name of lqr experiment folder') parser.add_argument('-gcm', '--gcm_experiment_folder', type=str, help='Name of gcm experiment folder') parser.add_argument('-badmm', '--badmm_experiment_folder', type=str, help='Name of badmm experiment folder') parser.add_argument('-i', '--iteration', type=int, help='Iteration to evaluate') parser.add_argument('-istart', '--startiteration', type=int, help='Start number of iterations') parser.add_argument('-iend', '--enditeration', type=int, help='Start number of iterations') args = parser.parse_args() from gps import __file__ as gps_filepath BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) cond = 0 policies = [] data_files_dirs = [] target_states = [] if args.lqr_experiment_folder is not None: policies.append('lqr') data_files_dirs.append(BASE_DIR + '/../experiments/' + args.lqr_experiment_folder + '/data_files/') lqr_target_filename = BASE_DIR + '/../experiments/' + args.lqr_experiment_folder + '/target.npz' lqr_target_pos, _, _ = load_pose_from_npz(lqr_target_filename, 'trial_arm', str(cond), 'target') lqr_target_vel = np.zeros(6) lqr_target_state = np.zeros(12) lqr_target_state[:6] = lqr_target_pos lqr_target_state[6:12] = lqr_target_vel target_states.append(lqr_target_state) if args.gcm_experiment_folder is not None: policies.append('mdggcs') data_files_dirs.append(BASE_DIR + '/../experiments/' + args.gcm_experiment_folder + '/data_files/') gcm_target_filename = BASE_DIR + '/../experiments/' + args.gcm_experiment_folder + '/target.npz' gcm_target_pos, _, _ = load_pose_from_npz(gcm_target_filename, 'trial_arm', str(cond), 'target') gcm_target_vel = np.zeros(6) gcm_target_state = np.zeros(12) gcm_target_state[:6] = gcm_target_pos gcm_target_state[6:12] = gcm_target_vel target_states.append(gcm_target_state) if args.badmm_experiment_folder is not None: policies.append('mdgps') data_files_dirs.append(BASE_DIR + '/../experiments/' + args.badmm_experiment_folder + '/data_files/') badmm_target_filename = BASE_DIR + '/../experiments/' + args.badmm_experiment_folder + '/target.npz' badmm_target_pos, _, _ = load_pose_from_npz(badmm_target_filename, 'trial_arm', str(cond), 'target') badmm_target_vel = np.zeros(6) badmm_target_state = np.zeros(12) badmm_target_state[:6] = badmm_target_pos badmm_target_state[6:12] = badmm_target_vel target_states.append(badmm_target_state) data_logger = DataLogger() description_line = '' for pol in policies: description_line += ', ' + pol + '_min_distance, ' + pol + '_last_distance, ' + pol + '_summed_distance' print(description_line[2:]) if args.iteration is not None: itr_idx = [args.iteration] else: itr_idx = range(args.startiteration, args.enditeration) for itr in itr_idx: line = '' for p in range(len(policies)): sample_lists = data_logger.unpickle(data_files_dirs[p] + 'pol_' + policies[p] + ('_sample_itr_%02d.pkl' % itr)) dT = len(sample_lists[cond].get_X()[0]) distances = [ euclidean_distance(sample_lists[cond].get_X()[0][i], target_states[p]) for i in range(dT) ] min_distance = min(distances) last_distance = distances[-1] summed_distance = sum(distances) #print("len: ",len(sample_lists[cond].get_EEF_Position()[0])) t_x = [ sample_lists[cond].get_EEF_Position()[0][i][0] for i in range(dT) ] t_y = [ sample_lists[cond].get_EEF_Position()[0][i][1] for i in range(dT) ] t_z = [ sample_lists[cond].get_EEF_Position()[0][i][2] for i in range(dT) ] #plot_trajectory(t_x,t_y,t_z) line += ', ' + str(min_distance) + ', ' + str( last_distance) + ', ' + str(summed_distance) print(line[2:])