def fit_spline_to_stream(strm, nsteps, deg=3):
    x = []
    y = []

    prev_rpy = None
    for i in xrange(nsteps):
        next = soft_next(strm)
        if next is not None:
            pos = next[0:3,3]
            rpy = np.array(tfms.euler_from_matrix(next), ndmin=2).T
            rpy = np.squeeze(rpy)
            if prev_rpy is not None:
                rpy = closer_angle(rpy, prev_rpy)
            prev_rpy = rpy

            x.append(i)
            y.append(pos.tolist()+rpy.tolist())

    x = np.asarray(x)
    y = np.asarray(y)

    s = len(x)*.001**2
    (tck, _) = si.splprep(y.T, s=s, u=x, k=deg)
    
    new_x = xrange(nsteps)
    xyzrpys = np.r_[si.splev(new_x, tck)].T
    
    smooth_tfms = []
    for xyzrpy in xyzrpys:
        tfm = tfms.euler_matrix(*xyzrpy[3:6])
        tfm[0:3,3] = xyzrpy[0:3]
        smooth_tfms.append(tfm)
        
    return smooth_tfms
def rpy2axang(rpy):
    """
    Converts a matrix of rpy (nx3) into a matrix of 
    axis-angles (nx3). 
    """
    n = rpy.shape[0]
    assert rpy.shape[1]==3, "unknown shape."
    ax_ang = np.empty((n,3))
    for i in xrange(n):
        th = rpy[i,:]
        ax_ang[i,:] = rave.axisAngleFromRotationMatrix(tfms.euler_matrix(th[0], th[1], th[2]))
    return ax_ang
def x_to_tf(Xs):
    """
    Converts a list of 12 dof state vector to a list of transforms.
    """
    Ts = []
    for x in Xs:
        trans = x[0:3]
        rot   = x[6:9]
        T = tfms.euler_matrix(rot[0], rot[1], rot[2])
        T[0:3,3] = np.reshape(trans, 3)
        Ts.append(T)
    return Ts
def add_noise(Ts, rotn = 5, xn = 0.0):
    """
    Adds noise to each transform in the list of transforms.
    rotn : standard dev of roll,pitch,yaw noise
    xn   : standard dev of translation noise 
    """
    rotn = np.deg2rad(rotn)
    Tn = []
    for t in Ts:
        rpy = np.array(tfms.euler_from_matrix(t))
        rpy += rotn*np.random.randn(3)
        tn  = tfms.euler_matrix(rpy[0], rpy[1], rpy[2]) 
        tn[0:3,3] = t[0:3,3] + xn*np.random.randn(3)
        Tn.append(tn)
    return Tn
def fit_spline_to_tf_stream(strm, new_freq, deg=3):
    """
    Interpolates a stream of transforms using splines, such that 
    there is no "NONE" when sampling the stream at NEW_FREQ frequency.
    
    Returns a stream of transforms.
    """
    tfs, ts = strm.get_data()
    tstart  = strm.get_start_time()
    tmax    = ts[-1]
    
    ndt = 1./new_freq
    new_ts  = np.arange(tstart, tmax+ndt/4., ndt/2.) 

    ## get data in xyzrpy format (6xn) matrix:
    N = len(tfs)
    tf_dat = np.empty((6, N))
    
    tf_dat[0:3,0] = tfs[0][0:3,3]    
    tf_dat[3:6,0] = tfms.euler_from_matrix(tfs[0])
    for i in xrange(1,N):
        now_tf  = tfs[i]
        tf_dat[0:3,i] = now_tf[0:3,3]
        
        prev_rpy  = tf_dat[3:6,i-1]
        now_rpy   = tfms.euler_from_matrix(now_tf)
        now_rpy   = closer_angle(now_rpy, prev_rpy)
        
        tf_dat[3:6,i] = now_rpy

    blueprint("\t fitting spline to data (scipy) ..")
    s = N*.001**2
    (tck, _) = si.splprep(tf_dat, s=s, u=ts, k=deg)

    blueprint("\t evaluating spline at new time-stamps ..")
    interp_xyzrpys = np.r_[si.splev(new_ts, tck)].T

    smooth_tfms = []
    for xyzrpy in interp_xyzrpys:
        tfm = tfms.euler_matrix(*xyzrpy[3:6])
        tfm[0:3,3] = xyzrpy[0:3]
        smooth_tfms.append(tfm)
        
    return streamize(smooth_tfms, new_ts, new_freq, strm.favg, tstart)
def gen_tfms(n=100, f=30):
    dt = 1./f

    rax  = np.array((1,0,0))
    v_rx  = np.deg2rad(180)

    v_x   = 1
    xax = np.array((1,1,0)) 

    ts = dt*np.arange(n)
    Ts = []

    x_init = np.zeros(3)
    r_init = np.zeros(3)
    
    for t in ts:
        x = x_init  + t*v_x*xax
        r = r_init  + t*v_rx*rax

        T = tfms.euler_matrix(r[0], r[1], r[2])
        T[0:3,3] = x
        Ts.append(T)

    return Ts