def Jacobian(dh):
T = __ConstructTransform__(dh, rho)
h = T[0:3, 3]
t = __ExtractOrients__(T[0:3, 0:3])
zs = [np.dot(t, np.array([0, 0, 1, 1]))[0:3] for t in T]
On = np.dot(T[-1], np.array([0, 0, 0, 1]))
Os = [On - np.dot(T[i], np.array([0, 0, 0, 1])) for i in range(len(T))]
j = []
for l in range(len(zs)):
j += [
np.concatenate(np.cross(zs[l], Os[l]), zs[l], axis=0)
if rho[l] == 1 else np.concatenate(zs[l], np.zeros([3, 1]), axis=0)
]
return np.matrix(j)
def get_order_boundary_indices(s, s0=None):
#x = np.arange(len(s))
s = np.array(s)
mm = s > max(s) * 0.05
dd1, dd2 = np.nonzero(mm)[0][[0, -1]]
if s0 is None:
s0 = get_smoothed_order_spec(s)
# mask out absorption feature
smooth_size=20
#s_s0 = s-s0
#s_s0_std = s_s0[np.abs(s_s0) < 2.*s_s0.std()].std()
#mmm = s_s0 > -3.*s_s0_std
s1 = ni.gaussian_filter1d(s0[dd1:dd2], smooth_size, order=1)
#x1 = x[dd1:dd2]
s1r = s1
s1_std = s1r.std()
s1_std = s1r[np.abs(s1r)<2.*s1_std].std()
s1r[np.abs(s1r) < 2.*s1_std] = np.nan
if np.any(np.isfinite(s1r[:1024])):
i1 = np.nanargmax(s1r[:1024])
i1r = np.where(~np.isfinite(s1r[:1024][i1:]))[0][0]
i1 = dd1+i1+i1r #+smooth_size
else:
i1 = dd1
if np.any(np.isfinite(s1r[1024:])):
i2 = np.nanargmin(s1r[1024:])
i2r = np.where(~np.isfinite(s1r[1024:][:i2]))[0][-1]
i2 = dd1+1024+i2r
else:
i2 = dd2
return i1, i2
def get_order_boundary_indices(s, s0=None):
#x = np.arange(len(s))
s = np.array(s)
mm = s > max(s) * 0.05
dd1, dd2 = np.nonzero(mm)[0][[0, -1]]
if s0 is None:
s0 = get_smoothed_order_spec(s)
# mask out absorption feature
smooth_size = 20
#s_s0 = s-s0
#s_s0_std = s_s0[np.abs(s_s0) < 2.*s_s0.std()].std()
#mmm = s_s0 > -3.*s_s0_std
s1 = ni.gaussian_filter1d(s0[dd1:dd2], smooth_size, order=1)
#x1 = x[dd1:dd2]
s1r = s1
s1_std = s1r.std()
s1_std = s1r[np.abs(s1r) < 2. * s1_std].std()
s1r[np.abs(s1r) < 2. * s1_std] = np.nan
if np.any(np.isfinite(s1r[:1024])):
i1 = np.nanargmax(s1r[:1024])
i1r = np.where(~np.isfinite(s1r[:1024][i1:]))[0][0]
i1 = dd1 + i1 + i1r #+smooth_size
else:
i1 = dd1
if np.any(np.isfinite(s1r[1024:])):
i2 = np.nanargmin(s1r[1024:])
i2r = np.where(~np.isfinite(s1r[1024:][:i2]))[0][-1]
i2 = dd1 + 1024 + i2r
else:
i2 = dd2
return i1, i2
def __ExtractOrients__(R):
t1 = m.atan2(R[2, 1], R[2, 2])
t2 = m.atan2(-R[2, 0], m.sqrt(R[2, 1]**2 + R[2, 2]**2))
t3 = m.atan2(R[1, 0], R[0, 0])
return np.array([t1, t2, t3])
def check_order_trace1(ax, x, s, i1i2):
x = np.arange(len(s))
ax.plot(x, s)
i1, i2 = i1i2
ax.plot(np.array(x)[[i1, i2]], np.array(s)[[i1,i2]], "o")
def check_order_trace1(ax, x, s, i1i2):
x = np.arange(len(s))
ax.plot(x, s)
i1, i2 = i1i2
ax.plot(np.array(x)[[i1, i2]], np.array(s)[[i1, i2]], "o")