def load_data():
dt = 1./30.
## load pr2-hydra calib data:
dat = cPickle.load(open(hd_path + '/hd_track/data/old/pr2-hydra-kinect-trajectory-transforms.cpickle'))
Ts_bh = dat['Ts_bh']
Ts_bg = dat['Ts_bg']
T_gh = dat['T_gh']
assert len(Ts_bg) == len(Ts_bh), "Number of hydra and pr2 transforms not equal."
Ts_bg_gh = [t.dot(T_gh) for t in Ts_bg]
X_bh = state_from_tfms(Ts_bh, dt).T
X_bg_gh = state_from_tfms(Ts_bg_gh, dt).T
X_bg_gh[6:9,:] = closer_angle(X_bg_gh[6:9,:], X_bh[6:9,:])
return (Ts_bh, Ts_bg, T_gh, Ts_bg_gh, X_bh, X_bg_gh)
def plot_complete():
"""
Plots the complete data : hydra, pr2, marker.
"""
dt = 1./30.
## load pr2-hydra calib data:
dat = cPickle.load(hd_path + '/hd_track/data/pr2-hydra-kinect-trajectory-transforms.cpickle')
Ts_bh = dat['Ts_bh']
Ts_bg = dat['Ts_bg']
T_gh = dat['T_gh']
assert len(Ts_bg) == len(Ts_bh), "Number of hydra and pr2 transforms not equal."
Ts_bg_gh = [t.dot(T_gh) for t in Ts_bg]
X_bh = state_from_tfms(Ts_bh, dt).T
X_bg_gh = state_from_tfms(Ts_bg_gh, dt).T
X_bg_gh[6:9,:] = closer_angle(X_bg_gh[6:9,:], X_bh[6:9,:])
return (Ts_bh, Ts_bg, T_gh, Ts_bg_gh, X_bh, X_bg_gh)
def load_data():
dt = 1./30.
## load pr2-hydra calib data:
dat = cPickle.load(open(hd_path + '/hd_track/data/nodup-transforms-1.cpickle'))
Ts_bh = dat['Ts_bh']
Ts_bg = dat['Ts_bg']
Ts_ba = dat['Ts_ba']
T_gh = dat['T_gh']
T_ga = dat['T_ga']
assert len(Ts_bg) == len(Ts_bh), "Number of hydra and pr2 transforms not equal."
Ts_bh_hg = [t.dot(np.linalg.inv(T_gh)) for t in Ts_bh]
Ts_ba_ag = []
ar_valid_stamps = []
Ts_ba_ag_t = []
for i in xrange(len(Ts_ba)):
t = Ts_ba[i]
if t == None:
Ts_ba_ag.append(None)
else:
Ts_ba_ag.append(t.dot(np.linalg.inv(T_ga)))
Ts_ba_ag_t.append(t.dot(np.linalg.inv(T_ga)))
ar_valid_stamps.append(i)
X_bg = state_from_tfms(Ts_bg, dt).T
X_bh_hg = state_from_tfms(Ts_bh_hg, dt).T
X_ba_ag_t = state_from_tfms_no_velocity(Ts_ba_ag_t).T
ar_valid_stamps = ar_valid_stamps[1:]
print X_bg.shape
print X_ba_ag_t.shape
print len(ar_valid_stamps)
X_bh_hg[6:9,:] = closer_angle(X_bh_hg[6:9,:], X_bg[6:9,:])
return (Ts_bh, Ts_bg, T_gh, Ts_bh_hg, X_bg, X_bh_hg, Ts_ba_ag, X_ba_ag_t, ar_valid_stamps)