def compute_mean_std(pkls, bin_size):
c_list = []
f_list = []
max_config = math.radians(100.)
typ = 'rotary'
for pkl_nm in pkls:
pull_dict = ut.load_pickle(pkl_nm)
pr2_log = 'pr2' in pkl_nm
h_config, h_ftan = force_trajectory_in_hindsight(pull_dict,
typ, pr2_log)
#h_config, h_ftan = online_force_with_radius(pull_dict, pr2_log)
c_list.append(h_config)
f_list.append(h_ftan)
max_config = min(max_config, np.max(h_config))
leng = int (max_config / bin_size) - 1
ff = []
for c, f in zip(c_list, f_list):
#c, f = maa.bin(c, f, bin_size, max, True)
c, f = maa.bin(c, f, bin_size, np.mean, False, empty_value = np.nan)
f, c = truncate_to_config(f, c, max_config)
f = np.ma.masked_array(f, np.isnan(f))
f = f[:leng]
c = c[:leng]
ff.append(f)
arr = np.array(ff)
mean = arr.mean(0)
std = arr.std(0)
return mean, std, c, arr
def compute_mean_std(pkls, bin_size):
c_list = []
f_list = []
max_config = math.radians(100.)
typ = 'rotary'
for pkl_nm in pkls:
pull_dict = ut.load_pickle(pkl_nm)
pr2_log = 'pr2' in pkl_nm
h_config, h_ftan = force_trajectory_in_hindsight(
pull_dict, typ, pr2_log)
#h_config, h_ftan = online_force_with_radius(pull_dict, pr2_log)
c_list.append(h_config)
f_list.append(h_ftan)
max_config = min(max_config, np.max(h_config))
leng = int(max_config / bin_size) - 1
ff = []
for c, f in zip(c_list, f_list):
#c, f = maa.bin(c, f, bin_size, max, True)
c, f = maa.bin(c, f, bin_size, np.mean, False, empty_value=np.nan)
f, c = truncate_to_config(f, c, max_config)
f = np.ma.masked_array(f, np.isnan(f))
f = f[:leng]
c = c[:leng]
ff.append(f)
arr = np.array(ff)
mean = arr.mean(0)
std = arr.std(0)
return mean, std, c, arr
def plot_trial(pkl_nm, max_ang, start_idx=None, mech_idx=None,
class_idx=None, start=None, mech=None, sem=None):
pull_dict = ut.load_pickle(pkl_nm)
typ = 'rotary'
pr2_log = 'pr2' in pkl_nm
h_config, h_ftan = atr.force_trajectory_in_hindsight(pull_dict,
typ, pr2_log)
h_config = np.array(h_config)
h_ftan = np.array(h_ftan)
h_ftan = h_ftan[h_config < max_ang]
h_config = h_config[h_config < max_ang] # cut
bin_size = math.radians(1.)
h_config_degrees = np.degrees(h_config)
ftan_raw = h_ftan
# resampling with specific interval
h_config, h_ftan = maa.bin(h_config, h_ftan, bin_size, np.mean, True)
pp.plot(np.degrees(h_config), h_ftan, 'yo-', mew=0, ms=0,
label='applied force', linewidth=2)
## pp.xlabel('Angle (degrees)')
## pp.ylabel('Opening Force (N)')
if start != None:
x = start[0]
y = start[1]
pp.plot([x], [y], 'ko', mew=0, ms=5)
#pp.text(x, y-1.5, '1', withdash=True) # cody_kitchen_box
#pp.text(x-1, y+1.0, '1', withdash=True) # cody_fridge_box
#pp.text(x, y-4., '1', withdash=True) # cody_fridge_chair
#pp.text(x+1.0, y-0.5, '1', withdash=True) # locked_pr2
#pp.text(x+1.0, y-0.5, '1', withdash=True) # locked_cody
x = mech[0]
y = mech[1]
pp.plot([x], [y], 'ko', mew=0, ms=5)
x = sem[0]
y = sem[1]
pp.plot([x], [y], 'ko', mew=0, ms=5)
if start_idx != None:
pp.plot([h_config_degrees[start_idx]], [ftan_raw[start_idx]],
'ko', mew=0, ms=5)
pp.plot([h_config_degrees[mech_idx]], [ftan_raw[mech_idx]],
'ko', mew=0, ms=5)
pp.plot([h_config_degrees[class_idx]], [ftan_raw[class_idx]],
'ko', mew=0, ms=5)
print 'Time with mechanism known:', (mech_idx-start_idx) * 100.
print 'Time with class known:', (class_idx-start_idx) * 100.
print ''
print 'Force increase with known mechanism:', ftan_raw[mech_idx] - ftan_raw[start_idx]
print 'Force increase with known class:', ftan_raw[class_idx] - ftan_raw[start_idx]
print ''
print 'Angle increase with known mechanism:', h_config_degrees[mech_idx] - h_config_degrees[start_idx]
print 'Angle increase with known class:', h_config_degrees[class_idx] - h_config_degrees[start_idx]
def simulate_perception(pkls, percep_std, name):
c_list = []
f_list = []
trials_per_pkl = 5
bin_size = math.radians(1.)
max_config = math.radians(100.)
for pkl_nm in pkls:
pull_dict = ut.load_pickle(pkl_nm)
pr2_log = 'pr2' in pkl_nm
for t in range(trials_per_pkl):
radius_err = np.random.normal(scale=percep_std)
#radius_err = np.random.uniform(-percep_std, percep_std)
h_config, h_ftan = online_force_with_radius(
pull_dict, pr2_log, radius_err)
c_list.append(h_config)
f_list.append(h_ftan)
max_config = min(max_config, np.max(h_config))
leng = int(max_config / bin_size) - 1
ff = []
for c, f in zip(c_list, f_list):
c, f = maa.bin(c, f, bin_size, np.mean, False, empty_value=np.nan)
f, c = truncate_to_config(f, c, max_config)
f = np.ma.masked_array(f, np.isnan(f))
f = f[:leng]
c = c[:leng]
ff.append(f)
arr = np.array(ff)
mean = arr.mean(0)
std = arr.std(0)
d = {}
d['std'] = std
d['mean'] = mean
d['rad'] = -3.141592
d['name'] = name
d['config'] = c
d['vec_list'] = arr.tolist()
d['typ'] = 'rotary'
ut.save_pickle(d, name + '.pkl')
def simulate_perception(pkls, percep_std, name):
c_list = []
f_list = []
trials_per_pkl = 5
bin_size = math.radians(1.)
max_config = math.radians(100.)
for pkl_nm in pkls:
pull_dict = ut.load_pickle(pkl_nm)
pr2_log = 'pr2' in pkl_nm
for t in range(trials_per_pkl):
radius_err = np.random.normal(scale=percep_std)
#radius_err = np.random.uniform(-percep_std, percep_std)
h_config, h_ftan = online_force_with_radius(pull_dict, pr2_log, radius_err)
c_list.append(h_config)
f_list.append(h_ftan)
max_config = min(max_config, np.max(h_config))
leng = int (max_config / bin_size) - 1
ff = []
for c, f in zip(c_list, f_list):
c, f = maa.bin(c, f, bin_size, np.mean, False, empty_value = np.nan)
f, c = truncate_to_config(f, c, max_config)
f = np.ma.masked_array(f, np.isnan(f))
f = f[:leng]
c = c[:leng]
ff.append(f)
arr = np.array(ff)
mean = arr.mean(0)
std = arr.std(0)
d = {}
d['std'] = std
d['mean'] = mean
d['rad'] = -3.141592
d['name'] = name
d['config'] = c
d['vec_list'] = arr.tolist()
d['typ'] = 'rotary'
ut.save_pickle(d, name+'.pkl')
def plot_trial(pkl_nm,
max_ang,
start_idx=None,
mech_idx=None,
class_idx=None,
start=None,
mech=None,
sem=None):
pull_dict = ut.load_pickle(pkl_nm)
typ = 'rotary'
pr2_log = 'pr2' in pkl_nm
h_config, h_ftan = atr.force_trajectory_in_hindsight(
pull_dict, typ, pr2_log)
h_config = np.array(h_config)
h_ftan = np.array(h_ftan)
h_ftan = h_ftan[h_config < max_ang]
h_config = h_config[h_config < max_ang] # cut
bin_size = math.radians(1.)
h_config_degrees = np.degrees(h_config)
ftan_raw = h_ftan
# resampling with specific interval
h_config, h_ftan = maa.bin(h_config, h_ftan, bin_size, np.mean, True)
pp.plot(np.degrees(h_config),
h_ftan,
'yo-',
mew=0,
ms=0,
label='applied force',
linewidth=2)
## pp.xlabel('Angle (degrees)')
## pp.ylabel('Opening Force (N)')
if start != None:
x = start[0]
y = start[1]
pp.plot([x], [y], 'ko', mew=0, ms=5)
#pp.text(x, y-1.5, '1', withdash=True) # cody_kitchen_box
#pp.text(x-1, y+1.0, '1', withdash=True) # cody_fridge_box
#pp.text(x, y-4., '1', withdash=True) # cody_fridge_chair
#pp.text(x+1.0, y-0.5, '1', withdash=True) # locked_pr2
#pp.text(x+1.0, y-0.5, '1', withdash=True) # locked_cody
x = mech[0]
y = mech[1]
pp.plot([x], [y], 'ko', mew=0, ms=5)
x = sem[0]
y = sem[1]
pp.plot([x], [y], 'ko', mew=0, ms=5)
if start_idx != None:
pp.plot([h_config_degrees[start_idx]], [ftan_raw[start_idx]],
'ko',
mew=0,
ms=5)
pp.plot([h_config_degrees[mech_idx]], [ftan_raw[mech_idx]],
'ko',
mew=0,
ms=5)
pp.plot([h_config_degrees[class_idx]], [ftan_raw[class_idx]],
'ko',
mew=0,
ms=5)
print 'Time with mechanism known:', (mech_idx - start_idx) * 100.
print 'Time with class known:', (class_idx - start_idx) * 100.
print ''
print 'Force increase with known mechanism:', ftan_raw[
mech_idx] - ftan_raw[start_idx]
print 'Force increase with known class:', ftan_raw[
class_idx] - ftan_raw[start_idx]
print ''
print 'Angle increase with known mechanism:', h_config_degrees[
mech_idx] - h_config_degrees[start_idx]
print 'Angle increase with known class:', h_config_degrees[
class_idx] - h_config_degrees[start_idx]
def plot_pkl(pkl_nm):
pull_dict = ut.load_pickle(pkl_nm)
if 'pr2' in pkl_nm:
pr2_log = True
h_color = 'y'
else:
pr2_log = False
h_color = 'r'
t = load_ref_traj(pkl_nm)
if t !=None:
ref_mean, ref_std, ref_config, typ = t
mar.plot_reference_trajectory(ref_config, ref_mean, ref_std, typ, 'Hello')
ref_config = np.degrees(ref_config)
max_config = np.max(ref_config)
else:
typ = 'rotary'
max_config = 60.
if pr2_log:
o_ftan = pull_dict['ftan_list']
o_config = pull_dict['config_list']
else:
o_ftan = pull_dict['online_ftan']
o_config = pull_dict['online_ang']
h_config, h_ftan = force_trajectory_in_hindsight(pull_dict,
typ, pr2_log)
if typ == 'rotary':
if opt.prior:
r_config, r_ftan = online_force_with_radius(pull_dict,
pr2_log)
r_config = np.degrees(r_config)
o_config = np.degrees(o_config)
h_config = np.degrees(h_config)
o_ftan, o_config = truncate_to_config(o_ftan, o_config, max_config)
h_ftan, h_config = truncate_to_config(h_ftan, h_config, max_config)
if typ == 'rotary':
if opt.prior:
r_ftan, r_config = truncate_to_config(r_ftan, r_config, max_config)
bin_size = 1.
else:
bin_size = 0.01
#o_config, o_ftan = maa.bin(o_config, o_ftan, bin_size, np.mean, True)
#h_config, h_ftan = maa.bin(h_config, h_ftan, bin_size, np.mean, True)
# non_nans = ~np.isnan(h_ftan)
# h_ftan = np.array(h_ftan)[non_nans]
# h_config = np.array(h_config)[non_nans]
#
# non_nans = ~np.isnan(o_ftan)
# o_ftan = np.array(o_ftan)[non_nans]
# o_config = np.array(o_config)[non_nans]
# h_config = h_config[:-1]
# h_ftan = h_ftan[1:]
if not pr2_log:
m,c = get_cody_calibration()
o_ftan = (np.array(o_ftan) - c) / m
h_ftan = (np.array(h_ftan) - c) / m
pp.plot(o_config, o_ftan, 'bo-', ms=5, label='online')
pp.plot(h_config, h_ftan, h_color+'o-', ms=5, label='hindsight')
if typ == 'rotary':
if opt.prior:
r_config, r_ftan = maa.bin(r_config, r_ftan, bin_size, max, True)
pp.plot(r_config, r_ftan, 'go-', ms=5, label='online with priors')
pp.xlabel('Configuration')
pp.ylabel('Tangential Force')
if pr2_log:
pp.figure()
p_list, f_list = pull_dict['ee_list'], pull_dict['f_list']
p_list = p_list[::2]
f_list = f_list[::2]
x_l, y_l, z_l = zip(*p_list)
pp.plot(x_l, y_l)
r, cx, cy = estimate_mechanism_kinematics(pull_dict, pr2_log)
mpu.plot_circle(cx, cy, r, 0., math.pi/2,
label='Actual\_opt', color='r')
pp.axis('equal')
def plot_pkl(pkl_nm):
pull_dict = ut.load_pickle(pkl_nm)
if 'pr2' in pkl_nm:
pr2_log = True
h_color = 'y'
else:
pr2_log = False
h_color = 'r'
t = load_ref_traj(pkl_nm)
if t != None:
ref_mean, ref_std, ref_config, typ = t
mar.plot_reference_trajectory(ref_config, ref_mean, ref_std, typ,
'Hello')
ref_config = np.degrees(ref_config)
max_config = np.max(ref_config)
else:
typ = 'rotary'
max_config = 60.
if pr2_log:
o_ftan = pull_dict['ftan_list']
o_config = pull_dict['config_list']
else:
o_ftan = pull_dict['online_ftan']
o_config = pull_dict['online_ang']
h_config, h_ftan = force_trajectory_in_hindsight(pull_dict, typ, pr2_log)
if typ == 'rotary':
if opt.prior:
r_config, r_ftan = online_force_with_radius(pull_dict, pr2_log)
r_config = np.degrees(r_config)
o_config = np.degrees(o_config)
h_config = np.degrees(h_config)
o_ftan, o_config = truncate_to_config(o_ftan, o_config, max_config)
h_ftan, h_config = truncate_to_config(h_ftan, h_config, max_config)
if typ == 'rotary':
if opt.prior:
r_ftan, r_config = truncate_to_config(r_ftan, r_config, max_config)
bin_size = 1.
else:
bin_size = 0.01
#o_config, o_ftan = maa.bin(o_config, o_ftan, bin_size, np.mean, True)
#h_config, h_ftan = maa.bin(h_config, h_ftan, bin_size, np.mean, True)
# non_nans = ~np.isnan(h_ftan)
# h_ftan = np.array(h_ftan)[non_nans]
# h_config = np.array(h_config)[non_nans]
#
# non_nans = ~np.isnan(o_ftan)
# o_ftan = np.array(o_ftan)[non_nans]
# o_config = np.array(o_config)[non_nans]
# h_config = h_config[:-1]
# h_ftan = h_ftan[1:]
if not pr2_log:
m, c = get_cody_calibration()
o_ftan = (np.array(o_ftan) - c) / m
h_ftan = (np.array(h_ftan) - c) / m
pp.plot(o_config, o_ftan, 'bo-', ms=5, label='online')
pp.plot(h_config, h_ftan, h_color + 'o-', ms=5, label='hindsight')
if typ == 'rotary':
if opt.prior:
r_config, r_ftan = maa.bin(r_config, r_ftan, bin_size, max, True)
pp.plot(r_config, r_ftan, 'go-', ms=5, label='online with priors')
pp.xlabel('Configuration')
pp.ylabel('Tangential Force')
if pr2_log:
pp.figure()
p_list, f_list = pull_dict['ee_list'], pull_dict['f_list']
p_list = p_list[::2]
f_list = f_list[::2]
x_l, y_l, z_l = zip(*p_list)
pp.plot(x_l, y_l)
r, cx, cy = estimate_mechanism_kinematics(pull_dict, pr2_log)
mpu.plot_circle(cx,
cy,
r,
0.,
math.pi / 2,
label='Actual\_opt',
color='r')
pp.axis('equal')