コード例 #1
0
def unif_resample(traj, max_diff, wt=None):
    """
    Resample a trajectory so steps have same length in joint space    
    """
    import scipy.interpolate as si
    tol = .005
    if wt is not None:
        wt = np.atleast_2d(wt)
        traj = traj * wt

    dl = mu.norms(traj[1:] - traj[:-1], 1)
    l = np.cumsum(np.r_[0, dl])
    goodinds = np.r_[True, dl > 1e-8]
    deg = min(3, sum(goodinds) - 1)
    if deg < 1: return traj, np.arange(len(traj))

    nsteps = max(int(np.ceil(float(l[-1]) / max_diff)), 2)
    newl = np.linspace(0, l[-1], nsteps)

    ncols = traj.shape[1]
    colstep = 10
    traj_rs = np.empty((nsteps, ncols))
    for istart in xrange(0, traj.shape[1], colstep):
        (tck, _) = si.splprep(traj[goodinds, istart:istart + colstep].T,
                              k=deg,
                              s=tol**2 * len(traj),
                              u=l[goodinds])
        traj_rs[:, istart:istart + colstep] = np.array(si.splev(newl, tck)).T
    if wt is not None: traj_rs = traj_rs / wt

    newt = np.interp(newl, l, np.arange(len(traj)))

    return traj_rs, newt
コード例 #2
0
ファイル: sim_util.py プロジェクト: amoliu/lfd
def unif_resample(traj, max_diff, wt = None):        
    """
    Resample a trajectory so steps have same length in joint space    
    """
    import scipy.interpolate as si
    tol = .005
    if wt is not None: 
        wt = np.atleast_2d(wt)
        traj = traj*wt
        
    dl = mu.norms(traj[1:] - traj[:-1],1)
    l = np.cumsum(np.r_[0,dl])
    goodinds = np.r_[True, dl > 1e-8]
    deg = min(3, sum(goodinds) - 1)
    if deg < 1: return traj, np.arange(len(traj))
    
    nsteps = max(int(np.ceil(float(l[-1])/max_diff)), 2)
    newl = np.linspace(0,l[-1],nsteps)

    ncols = traj.shape[1]
    colstep = 10
    traj_rs = np.empty((nsteps,ncols)) 
    for istart in xrange(0, traj.shape[1], colstep):
        (tck,_) = si.splprep(traj[goodinds, istart:istart+colstep].T,k=deg,s = tol**2*len(traj),u=l[goodinds])
        traj_rs[:,istart:istart+colstep] = np.array(si.splev(newl,tck)).T
    if wt is not None: traj_rs = traj_rs/wt

    newt = np.interp(newl, l, np.arange(len(traj)))

    return traj_rs, newt
コード例 #3
0
ファイル: kinematics_utils.py プロジェクト: zzz622848/lfd
def unif_resample(x, n, weights, tol=.001, deg=3):
    x = np.atleast_2d(x)
    weights = np.atleast_2d(weights)
    x = mu.remove_duplicate_rows(x)
    x_scaled = x * weights
    dl = mu.norms(x_scaled[1:] - x_scaled[:-1], 1)
    l = np.cumsum(np.r_[0, dl])
    (tck, _) = si.splprep(x_scaled.T, k=deg, s=tol**2 * len(x), u=l)

    newu = np.linspace(0, l[-1], n)
    out_scaled = np.array(si.splev(newu, tck)).T
    out = out_scaled / weights
    return out
コード例 #4
0
ファイル: kinematics_utils.py プロジェクト: amoliu/lfd
def unif_resample(x,n,weights,tol=.001,deg=3):    
    x = np.atleast_2d(x)
    weights = np.atleast_2d(weights)
    x = mu.remove_duplicate_rows(x)
    x_scaled = x * weights
    dl = mu.norms(x_scaled[1:] - x_scaled[:-1],1)
    l = np.cumsum(np.r_[0,dl])
    (tck,_) = si.splprep(x_scaled.T,k=deg,s = tol**2*len(x),u=l)
    
    newu = np.linspace(0,l[-1],n)
    out_scaled = np.array(si.splev(newu,tck)).T
    out = out_scaled/weights
    return out