'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:])