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)