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 adaptive_resample(x, tol, max_change=None, min_steps=3):
"""
resample original signal with a small number of waypoints so that the the sparsely sampled function,
when linearly interpolated, deviates from the original function by less than tol at every time
input:
x: 2D array in R^(t x k) where t is the number of timesteps
tol: tolerance. either a single scalar or a vector of length k
max_change: max change in the sparsely sampled signal at each timestep
min_steps: minimum number of timesteps in the new trajectory. (usually irrelevant)
output:
new_times, new_x
assuming that the old signal has times 0,1,2,...,len(x)-1
this gives the new times, and the new signal
"""
x = np.asarray(x)
assert x.ndim == 2
if np.isscalar(tol):
tol = np.ones(x.shape[1])*tol
else:
tol = np.asarray(tol)
assert tol.ndim == 1 and tol.shape[0] == x.shape[1]
times = np.arange(x.shape[0])
if max_change is None:
max_change = np.ones(x.shape[1]) * np.inf
elif np.isscalar(max_change):
max_change = np.ones(x.shape[1]) * max_change
else:
max_change = np.asarray(max_change)
assert max_change.ndim == 1 and max_change.shape[0] == x.shape[1]
dl = mu.norms(x[1:] - x[:-1],1)
l = np.cumsum(np.r_[0,dl])
def bad_inds(x1, t1):
ibad = np.flatnonzero( (np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1) )
jbad1 = np.flatnonzero((np.abs(x1[1:] - x1[:-1]) > max_change[None,:]).any(axis=1))
if len(ibad) == 0 and len(jbad1) == 0: return []
else:
lbad = l[ibad]
jbad = np.unique(np.searchsorted(l1, lbad)) - 1
jbad = np.union1d(jbad, jbad1)
return jbad
l1 = np.linspace(0,l[-1],min_steps)
for _ in xrange(20):
x1 = mu.interp2d(l1, l, x)
bi = bad_inds(x1, l1)
if len(bi) == 0:
return np.interp(l1, l, times), x1
else:
l1 = np.union1d(l1, (l1[bi] + l1[bi+1]) / 2 )
raise Exception("couldn't subdivide enough. something funny is going on. check your input data")
def unif_resample(x,n=None,tol=0,deg=None, seg_len = .02):
if deg is None: deg = min(3, len(x) - 1)
dl = mu.norms(x[1:] - x[:-1],1)
l = np.cumsum(np.r_[0,dl])
goodinds = np.r_[True, dl > 1e-8]
x = np.atleast_2d(x)
x = x[goodinds]
(tck,_) = si.splprep(x.T,k=deg,s = tol**2*len(x),u=np.linspace(0,1,len(x)))
xup = np.array(si.splev(np.linspace(0,1, 10*len(x),.1),tck)).T
dl = mu.norms(xup[1:] - xup[:-1],1)
l = np.cumsum(np.r_[0,dl])
(tck,_) = si.splprep(xup.T,k=deg,s = tol**2*len(xup),u=l)
if n is not None: newu = np.linspace(0,l[-1],n)
else: newu = np.linspace(0, l[-1], l[-1]//seg_len)
return np.array(si.splev(newu,tck)).T
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
def rope_len(x):
return mu.norms(x[1:] - x[:-1],1).sum()
