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
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
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
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
