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
