Ejemplo n.º 1
0
    '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(
Ejemplo n.º 2
0
    '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)
Ejemplo n.º 3
0
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:])