def compute_dR_dq(calc_source, sat, attitude, t):
"""
| Ref. Paper [LUDW2011]_ eq. [79].
| Computes the derivative of the cost-function w.r.t. quaternion q i.e. the
tuple of equations:
- :math:`\\frac{dR_l^{AL}}{dq_l}=2 \cdot sec(\zeta_l) q_l * \{S'n_l, 0\}` which
is Along_scan w.r.t. observation number l
- :math:`\\frac{dR_l^{AC}}{dq_l}=-2 q_l * \{S'm_l, 0\}`
where ``*`` is a quaternion multiplication
:param calc_source: [calc_source object]
:param sat: [sat object]
:param attitude: [numpy quaternion]
:param t: [float][days] time
:returns: [tuple of numpy arrays] (dR^AL/dq, dR^AC/dq)
"""
# Here below we have "phi" since we set double_telescope to False
phi, zeta = calculated_field_angles(calc_source,
attitude,
sat,
t,
double_telescope=False)
Sm, Sn, Su = compute_mnu(phi, zeta)
q = attitude
dR_dq_AL = 2 * helpers.sec(zeta) * (q * ft.vector_to_quat(Sn))
dR_dq_AC = -2 * (q * ft.vector_to_quat(Sm))
return (quaternion.as_float_array(dR_dq_AL),
quaternion.as_float_array(dR_dq_AC))
def find_BT_from_BT(bt_true, xp, yp, weights):
q = np.exp(np.quaternion(*bt_true[:3]))
t = np.quaternion(*bt_true[3:])
hdx_R, hdy_R, hnd_raw_R = get_hessian_parts_R(xp, yp)
r_x, r_y, hnd_R, l_x, l_y, Hdx_R_inv, Hdy_R_inv, Hnd_R_inv = fast_findanalytic_R(
q, t, weights, xp, yp, hdx_R, hdy_R, hnd_raw_R)
x = np.transpose(r_x * np.transpose(xp))
y = np.transpose(r_y * np.transpose(yp))
#q, t, y = iterate_BT(x, y, weights)
q, t, y = iterate_BT(x, y, xp, yp, weights, q, t, r_y)
qf, tf, j, dLdg, dLdrx, dLdry, H_bt_inv, yf = fast_iterate_BT_newton(
x, y, xp, yp, weights, q, t, r_y)
dLdrH_inv_x = np.transpose(dLdrx) @ Hdx_R_inv + \
np.transpose(dLdry) @ np.transpose(Hnd_R_inv)
dLdrH_inv_y = np.transpose(dLdrx) @ Hnd_R_inv + \
np.transpose(dLdry) @ Hdy_R_inv
dLrg = - np.einsum('ij,k->jki', dLdrH_inv_x * (hdx_R * r_x), np.ones(len(yp))) \
- np.einsum('ij,jk->jki', dLdrH_inv_x, (hnd_R * r_y)) \
- np.einsum('ij,jk->kji', dLdrH_inv_y, (np.transpose(hnd_R) * r_x)) \
- np.einsum('ij,k->kji', dLdrH_inv_y * (hdy_R * r_y), np.ones(len(xp))) \
- np.einsum('ik,j->kji', dLdrH_inv_x * l_x, np.ones(len(yp))) \
- np.einsum('ij,k->kji', dLdrH_inv_y * l_y, np.ones(len(xp)))
dbt = np.einsum('ijk,km->ijm', dLdg + dLrg, -H_bt_inv @ j)
bt = np.concatenate((quaternion.as_float_array(np.log(qf))[1:],
quaternion.as_float_array(tf)[1:]))
return bt, dbt
def fast_findanalytic_R(q, t, weights, xp, yp, hdx_R, hdy_R, hnd_raw_R):
q = quaternion.as_float_array(q)
t = quaternion.as_float_array(t)[1:]
a = 2 * np.arccos(q[0])
if a != 0:
u = q[1:] / np.sin(a / 2)
else:
u = np.array([0, 0, 0])
angle_mat = (np.cos(a) - 1) * np.einsum('i,j->ij', u, u)\
+ np.sin(a) * np.einsum('ijk,k->ij', np.array([[[LeviCivita(i, j, k) for k in range(3)] for j in range(3)] for i in range(3)], dtype=np.double), u)\
- np.cos(a) * np.eye(3)
hnd_R = 2 * np.einsum('ijkl,kl->ij', hnd_raw_R, angle_mat)
Hdx_R = np.einsum('i,ij->i', hdx_R, weights)
Hdy_R = np.einsum('i,ji->i', hdy_R, weights)
#Hnd_R= np.array([hnd_R[f(ind)] *weights(ind) for ind,_ in np.denumerate(weights)])
Hnd_R = hnd_R * weights
Hnd_R_inv = (np.linalg.inv(((Hnd_R / Hdy_R) @ np.transpose(Hnd_R)) -
np.diag(Hdx_R)) @ Hnd_R) / Hdy_R
Hdy_R_inv = np.einsum('i,ij->ij', 1 / Hdy_R,
np.eye(len(xp)) - np.transpose(Hnd_R) @ Hnd_R_inv)
Hdx_R_inv = np.einsum('i,ij->ij', 1 / Hdx_R,
np.eye(len(xp)) - Hnd_R @ np.transpose(Hnd_R_inv))
l_x = 2 * np.einsum('ij,j->i', xp, t)
l_y_vec = t * np.cos(a) + (u @ t) * (1 - np.cos(a)) * \
u + np.sin(a) * np.cross(t, u)
l_y = -2 * np.einsum('ij,j->i', yp, l_y_vec)
L_x = np.einsum('ij,i->i', weights, l_x)
L_y = np.einsum('ji,i->i', weights, l_y)
r_x = -Hdx_R_inv @ L_x - Hnd_R_inv @ L_y
r_y = -L_x @ Hnd_R_inv - Hdy_R_inv @ L_y
return r_x, r_y, hnd_R, l_x, l_y, Hdx_R_inv, Hdy_R_inv, Hnd_R_inv
def rotations(self, theta, phi, alpha, beta, gamma):
"""
Return ligand atoms positions after rotation around receptor and self-rotation.
Rotation around receptor will be treated first, then self-rotation
The first rotation is caracterized by two parameters, theta and phi (Eular angles)
The self-rotation is caracterized by thee parameters, alpha, beta, and gamma.
Keyword arguments:
theta -- ligand's rotation angle around axis defined by z-axis X rec -> lig
phi -- ligand's rotation angle around z-axis
alpha -- self-rotation's angle around z-axis X (z-axis X lig -> rec)
beta -- self-rotation's angle around z-axis X lig -> rec
gamma -- self-rotation's angle around vector ligand -> receptor
"""
# TODO : Simplify
transq = rot_around_rec_quat(self.axis, theta, gamma)
tmp_axis_quat = transq * quat.quaternion(
*self.axis) * np.conjugate(transq)
tmp_axis = quat.as_float_array(tmp_axis_quat)[1:]
rotateq = self_rot_quat(-tmp_axis, alpha, beta, gamma)
trans_quat = transq * (self.lig_quat -
self.center_rec_quat) * np.conjugate(transq)
rotate_quat = rotateq * (trans_quat -
tmp_axis_quat) * np.conjugate(rotateq)
return quat.as_float_array(rotate_quat + tmp_axis_quat +
self.center_rec_quat)[:, 1:]
def createSequence(keyframes, dt=0.1):
timestamps = [frame.timestamp for frame in keyframes]
rotations = quaternion.as_quat_array([[frame.frame.orientation.x, frame.frame.orientation.y, frame.frame.orientation.z, frame.frame.orientation.w] for frame in keyframes])
# Coordinate frame conversion
multiplier_quat = quaternion.from_float_array([np.sqrt(2) / 2, -np.sqrt(2) / 2, 0, 0])
rotations = [rot * multiplier_quat for rot in rotations]
positions = np.array([[frame.frame.position.x, frame.frame.position.y, frame.frame.position.z] for frame in keyframes])
time_interps = list(np.arange(timestamps[0], timestamps[-1], dt)) + [timestamps[-1]]
lerp = interp1d(timestamps, positions, axis=0)
lin_interps = lerp(time_interps)
rot_interps = []
for i in range(len(keyframes) - 1):
rot_interps.extend(quaternion.as_float_array(quaternion.slerp(rotations[i], rotations[i + 1], timestamps[i], timestamps[i + 1], np.arange(timestamps[i], timestamps[i + 1], dt))))
rot_interps.append(quaternion.as_float_array(rotations[-1]))
sequence = []
for pos, ori in zip(lin_interps, rot_interps):
pose = Pose()
pose.position.x, pose.position.y, pose.position.z = pos
pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z = ori
pose_stamped = PoseStamped()
pose_stamped.pose = pose
sequence.append(pose_stamped)
g_poses[0] = sequence[-1].pose
g_poses[1] = sequence[-1].pose
return time_interps, sequence
def test_numpy_array_conversion(Qs):
"Check conversions between array as quaternions and array as floats"
# First, just check 1-d array
Q = Qs[Qs_nonnan][:12] # Select first 3x4=12 non-nan elements in Qs
assert Q.dtype == np.dtype(np.quaternion)
q = quaternion.as_float_array(Q) # View as array of floats
assert q.dtype == np.dtype(np.float)
assert q.shape == (12, 4) # This is the expected shape
for j in range(12):
for k in range(4): # Check each component individually
assert q[j][k] == Q[j].components[k]
assert np.array_equal(quaternion.as_quat_array(q), Q) # Check that we can go backwards
# Next, see how that works if I flatten the q array
q = q.flatten()
assert q.dtype == np.dtype(np.float)
assert q.shape == (48,)
for j in range(48):
assert q[j] == Q[j // 4].components[j % 4]
assert np.array_equal(quaternion.as_quat_array(q), Q) # Check that we can go backwards
# Finally, reshape into 2-d array, and re-check
P = Q.reshape(3, 4) # Reshape into 3x4 array of quaternions
p = quaternion.as_float_array(P) # View as array of floats
assert p.shape == (3, 4, 4) # This is the expected shape
for j in range(3):
for k in range(4):
for l in range(4): # Check each component individually
assert p[j][k][l] == Q[4 * j + k].components[l]
assert np.array_equal(quaternion.as_quat_array(p), P) # Check that we can go backwards
def quat_project(quat: q.quaternion, axis: np.ndarray) -> np.ndarray:
"""Project rotation in quat onto axis."""
axis_project = np.dot(q.as_float_array(quat)[1:], axis) * axis
quat_proj = np.quaternion(
q.as_float_array(quat)[0], axis_project[0], axis_project[1],
axis_project[2])
return quat_proj / np.absolute(quat_proj)
def angular_velocity(W, include_frame_velocity=False):
"""Angular velocity of Waveform
This function calculates the angular velocity of a Waveform object from
its modes. This was introduced in Sec. II of "Angular velocity of
gravitational radiation and the corotating frame"
<http://arxiv.org/abs/1302.2919>. Essentially, this is the angular
velocity of the rotating frame in which the time dependence of the modes
is minimized.
The vector is given in the (possibly rotating) mode frame (X,Y,Z),
which is not necessarily equal to the inertial frame (x,y,z).
"""
# Calculate the <Ldt> vector and <LL> matrix at each instant
l = W.LdtVector()
ll = W.LLMatrix()
# Solve <Ldt> = - <LL> . omega
omega = -np.linalg.solve(ll, l)
if include_frame_velocity and len(W.frame) == W.n_times:
from quaternion import derivative, as_quat_array, as_float_array
Rdot = as_quat_array(derivative(as_float_array(W.frame), W.t))
omega += as_float_array(2 * Rdot * W.frame.conjugate())[:, 1:]
return omega
def test_ndcomplex_swapdims_quaternion():
np.random.seed(12345)
d = np.random.random((4, 3)) * np.exp(.1j)
d3 = NDComplexArray(d, units=ur.Hz, mask=[[False, True, False], [True, False, False]],
dtype=typequaternion) # quaternion with units & mask
assert d3.shape == (2, 3)
assert d3._data.shape == (2, 3)
assert d3.has_complex_dims
assert d3.is_quaternion
w, x, y, z = as_float_array(d3.data).T
d4 = d3.swapdims(0, 1)
assert d4.shape == (3, 2)
assert d4._data.shape == (3, 2)
assert d4.has_complex_dims
assert d4.is_quaternion
wt, yt, xt, zt = as_float_array(d4.data).T
assert_array_equal(xt, x.T)
assert_array_equal(yt, y.T)
assert_array_equal(zt, z.T)
assert_array_equal(wt, w.T)
def angular_velocity(R, t):
from scipy.interpolate import CubicSpline
R = quaternion.as_float_array(R)
Rdot = CubicSpline(t, R).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
return quaternion.as_float_array(2*Rdot/R)[:, 1:]
def test_qinv(self):
""""""
q = from_euler_angles(roll=np.pi / 4, return_quaternion=True)
assert qinv(q) == 1 / q
npt.assert_equal(qinv(as_float_array(q)), as_float_array(1 / q))
q_arr = np.tile(as_float_array(q), (10, 1)).T
npt.assert_equal(qinv(q_arr, 0)[:, 0], as_float_array(1 / q))
def angular_velocity(R, t):
from scipy.interpolate import InterpolatedUnivariateSpline as spline
R = quaternion.as_float_array(R)
Rdot = np.empty_like(R)
for i in range(4):
Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
return quaternion.as_float_array(2*Rdot/R)[:, 1:]
def calc_rotation_diff(q, q00):
rotation_dot = np.dot(quaternion.as_float_array(q00), quaternion.as_float_array(q))
rotation_dot_abs = np.abs(rotation_dot)
try:
error_rotation_rad = 2 * np.arccos(rotation_dot_abs)
except:
return 0.0
error_rotation_rad = 2 * np.arccos(rotation_dot_abs)
error_rotation = np.rad2deg(error_rotation_rad)
return error_rotation
def from_dq_array(cls, r_wxyz_t_wxyz):
"""
Create a DualQuaternion instance from two quaternions in list format
:param r_wxyz_t_wxyz: np.array or python list: np.array([q_rw, q_rx, q_ry, q_rz, q_tx, q_ty, q_tz]
"""
r_wxyz_t_wxyz = np.asarray(r_wxyz_t_wxyz)
if r_wxyz_t_wxyz.ndim>1:
return cls(quaternion.as_float_array(r_wxyz_t_wxyz[:,:4]), quaternion.as_float_array(r_wxyz_t_wxyz[:,4:]))
else:
return cls(quaternion.as_float_array(r_wxyz_t_wxyz[:4]), quaternion.as_float_array(r_wxyz_t_wxyz[4:]))
def test2():
print("Test 2")
dmp = dmpy.QuaternionDMP(100)
data = np.load('trajectory.npz')
quats = quaternion.as_quat_array(data['quaternions'])
vel = data['vel_angular']
N = len(quats)
dt = 1 / 30
ts = np.linspace(0, dt * N, N)
ts_train = np.linspace(0, 1, N)
fig, (ax1, ax2, ax3) = plt.subplots(3, sharex=True)
ax1.plot(ts, quaternion.as_float_array(quats))
ax1.legend([r'$q_{}$'.format(i) for i in 'wxyz'], loc='upper right')
ax2.plot(ts, vel)
ax2.legend([r'$\omega_{}$'.format(i) for i in 'xyz'], loc='upper right')
ax3.plot(ts, np.gradient(vel, axis=0) / dt)
ax3.legend([r'$\dot\omega_{}$'.format(i) for i in 'xyz'],
loc='upper right')
fig.suptitle('Input trajectory')
dmp.train(quats, ts_train, 1.0)
fig, (ax1, ax2, ax3) = plt.subplots(3, sharex=True)
ax1.plot(ts_train, quaternion.as_float_array(dmp.train_quats))
ax1.legend([r'$q_{}$'.format(i) for i in 'wxyz'], loc='upper right')
ax2.plot(ts_train, dmp.train_omega)
ax2.legend([r'$\omega_{}$'.format(i) for i in 'xyz'], loc='upper right')
ax3.plot(ts_train, dmp.train_d_omega)
ax3.legend([r'$\dot\omega_{}$'.format(i) for i in 'xyz'],
loc='upper right')
fig.suptitle('Train trajectory')
q, omega, domega = dmp.rollout(ts)
# q, omega, domega = dmp.rollout(ts, ts[N//2])
# vartau = 40 + 10*np.sin(ts*0.2)
# plt.figure()
# plt.plot(ts, vartau)
# q, omega, domega = dmp.rollout(ts, vartau)
fig, (ax1, ax2, ax3) = plt.subplots(3, sharex=True)
ax1.plot(ts, quaternion.as_float_array(q))
ax1.legend([r'$q_{}$'.format(i) for i in 'wxyz'], loc='upper right')
ax2.plot(ts, omega)
ax2.legend([r'$\omega_{}$'.format(i) for i in 'xyz'], loc='upper right')
ax3.plot(ts, domega)
ax3.legend([r'$\dot\omega_{}$'.format(i) for i in 'xyz'],
loc='upper right')
fig.suptitle('DMP trajectory')
plt.show()
def fill_traj(traj, frames_to_avg, frames_to_fill, fill_down=True):
dt = traj.dt
# compute averages
ang_vel_avg_up = np.mean(traj.ang_vel[0:frames_to_avg, :], axis=0)
ang_vel_avg_up_angle = np.linalg.norm(ang_vel_avg_up)
ang_vel_avg_up_axis = ang_vel_avg_up / ang_vel_avg_up_angle
ang_vel_avg_down = np.mean(traj.ang_vel[-frames_to_avg:, :], axis=0)
ang_vel_avg_down_angle = np.linalg.norm(ang_vel_avg_down)
ang_vel_avg_down_axis = ang_vel_avg_down / ang_vel_avg_down_angle
vel_avg_up = np.mean(traj.vel[0:frames_to_avg, :], axis=0)
vel_avg_down = np.mean(traj.vel[-frames_to_avg:, :], axis=0)
# add additional frames
pos_up_filled = np.stack([
vel_avg_up * dt * i + traj.pos[0]
for i in range(-frames_to_fill, 0)
], 0)
pos_down_filled = np.stack([
vel_avg_down * dt * i + traj.pos[-1]
for i in range(1, frames_to_fill + 1)
], 0)
quat_up_filled = np.stack([
create_quat(ang_vel_avg_up_angle * dt * i, ang_vel_avg_up_axis) *
traj.quat[0] for i in range(-frames_to_fill, 0)
], 0)
quat_down_filled = np.stack([
create_quat(ang_vel_avg_down_angle * dt * i, ang_vel_avg_down_axis)
* traj.quat[-1] for i in range(1, frames_to_fill + 1)
], 0)
# create new trajectory
new_frame_nums = np.concatenate(
(np.arange(traj.frame_nums[0] - frames_to_fill,
traj.frame_nums[0]), traj.frame_nums,
np.arange(traj.frame_nums[-1] + 1,
traj.frame_nums[-1] + frames_to_fill + 1)))
if new_frame_nums[0] < 0:
new_frame_nums = new_frame_nums + (-new_frame_nums[0])
if fill_down:
pos = np.concatenate((pos_up_filled, traj.pos, pos_down_filled),
axis=0)
quat = q.as_float_array(
np.concatenate((quat_up_filled, traj.quat, quat_down_filled),
axis=0))
return PoseTrajectory.from_quat(pos, quat, dt, new_frame_nums)
else:
pos = np.concatenate((pos_up_filled, traj.pos), axis=0)
quat = q.as_float_array(
np.concatenate((quat_up_filled, traj.quat), axis=0))
return PoseTrajectory.from_quat(pos, quat, dt,
new_frame_nums[:-frames_to_fill])
def get_rs(q, t, weights_not_normd, xp, yp, hdx_R, hdy_R, hnd_raw_R):
weights = weights_not_normd * weights_not_normd / np.sum(
weights_not_normd * weights_not_normd)
q = quaternion.as_float_array(q)
t = quaternion.as_float_array(t)[1:]
a = 2 * np.arccos(q[0])
if a != 0:
u = q[1:] / np.sin(a / 2)
else:
u = np.array([0, 0, 0])
angle_mat = (np.cos(a) - 1) * np.einsum('i,j->ij', u, u)\
- np.sin(a) * np.einsum('ijk,k->ij', np.array([[[LeviCivita(i, j, k) for k in range(3)] for j in range(3)] for i in range(3)], dtype=np.double), u)\
- np.cos(a) * np.eye(3)
hnd_R = np.einsum('ijkl,kl->ij', hnd_raw_R, angle_mat)
Hdx_R = np.einsum('j,ij->j', hdx_R, weights)
Hdy_R = np.einsum('i,ij->i', hdy_R, weights)
#Hnd_R= np.array([hnd_R[f(ind)] *weights(ind) for ind,_ in np.denumerate(weights)])
Hnd_R = hnd_R * weights
l_x = np.einsum('ij,j->i', xp, t)
l_y_vec = t * np.cos(a) + (u@t) * (1 - np.cos(a)) * \
u + np.sin(a) * np.cross(t, u)
l_y = -np.einsum('ij,j->i', yp, l_y_vec)
L_x = np.einsum('ij,j->j', weights, l_x)
L_y = np.einsum('ij,i->i', weights, l_y)
hnd_inter = ((Hnd_R / Hdx_R) @ np.transpose(Hnd_R) - np.diag(Hdy_R))
inv = np.linalg.inv((Hnd_R / Hdx_R) @ np.transpose(Hnd_R) - np.diag(Hdy_R))
Y = inv @ Hnd_R / Hdx_R
ry = inv @ L_y - Y @ L_x
rx = np.diag(-1 / Hdx_R) @ np.transpose(Hnd_R) @ ry - L_x / Hdx_R
"""
#print('zero:',np.diag(Hdy_R) @ ry + Hnd_R @ rx + L_y)
#print('zero:', np.transpose(Hnd_R) @ ry + np.diag(Hdx_R) @ rx + L_x)
X = -inv
Z = np.diag(1 / Hdx_R) @ (np.eye(len(Hdx_R)) - np.transpose(Hnd_R) @ Y)
drydG = np.einsum('ij,j,j,k->ijk', X, hdy_R, -ry, np.ones_like(rx)) \
+ np.einsum('ij,jk,k->ijk', X, hnd_R, -rx) \
+ np.einsum('ik,jk,j->ijk', Y, hnd_R, -ry) \
+ np.einsum('ik,k,k,j->ijk', Y, hdx_R, -rx, np.ones_like(rx)) \
- np.einsum('ji,j,k->ijk', X, l_y, np.ones_like(rx)) \
- np.einsum('ik,k,j->ijk', Y, l_x, np.ones_like(rx))
drxdG = np.einsum('ji,j,j,k->ijk', Y, hdy_R, -ry, np.ones_like(rx)) \
+ np.einsum('ji,jk,k->ijk', Y, hnd_R, -rx) \
+ np.einsum('ik,jk,j->ijk', Z, hnd_R, -ry) \
+ np.einsum('ik,k,k,j->ijk', Z, hdx_R, -rx, np.ones_like(rx)) \
- np.einsum('ji,j,k->ijk', Y, l_y, np.ones_like(rx)) \
- np.einsum('ik,k,j->ijk', Z, l_x, np.ones_like(rx))
dG_dg = 2 / np.sum(weights_not_normd * weights_not_normd) * np.einsum('im,jn,ij->ijmn', np.eye(len(xp)), np.eye(len(xp)), weights_not_normd)\
- 2 / np.sum(weights_not_normd * weights_not_normd)** 2 * np.einsum('ij,mn->ijmn', weights_not_normd ** 2, weights_not_normd)
drxdg = np.einsum('ijk,jkmn->imn', drxdG, dG_dg)
drydg = np.einsum('ijk,jkmn->imn', drydG, dG_dg)
"""
return rx, ry
def load(self, data_path):
if data_path[-1] == '/':
data_path = data_path[:-1]
with open(osp.join(data_path, 'info.json')) as f:
self.info = json.load(f)
# {'tango_reference_time': 3697949488366.0, 'date': '01/21/19', 'imu_init_gyro_bias': [0.005569000000000001, 0.009201, -0.00018299999999999914], 'imu_acce_scale': [0.9987918889533104, 0.997062129866083, 0.9932574091553678], 'grv_ori_error': 8.353779492444412, 'align_tango_to_body': [-0.4841758535176575, -0.4938374978693588, -0.5424326634072199, 0.47693298715598254], 'start_frame': 5896, 'imu_end_gyro_bias': [0.005432, 0.008774, -4.6e-05], 'type': 'annotated', 'imu_reference_time': 3610942440968.0, 'start_calibration': [0.0, 0.99995545, 0.009439, 0.0], 'ekf_ori_error': 3.6239102945197676, 'imu_acce_bias': [-0.15661902624553775, -0.026333329541761968, 0.05681364453654479], 'gyro_integration_error': 8.481689436777705, 'device': 'asus4', 'length': 323.7550000070669, 'imu_time_offset': -0.07619643889847794, 'end_calibration': [0.0, 0.99998599, 0.0052938, 0.0]}
self.info['path'] = osp.split(data_path)[-1]
self.info['ori_source'], ori, self.info['source_ori_error'] = select_orientation_source( # ori is hdf5.synced.game_rv i. e. Android Sensor.TYPE_GAME_ROTATION_VECTOR
data_path, self.max_ori_error, self.grv_only)
#('game_rv', array([[ 0.02301384, -0.734161 , 0.00956859, 0.67851714], [ 0.02296023, -0.73417201, 0.00956628, 0.67850771], ..., [ 0.05427992, 0.70881762, 0.07637797, -0.6991363 ]]]), 8.353779492444412)
with h5py.File(osp.join(data_path, 'data.hdf5')) as f:
gyro_uncalib = f['synced/gyro_uncalib'] # <HDF5 dataset "gyro_uncalib": shape (64752, 3), type "<f8">
acce_uncalib = f['synced/acce'] # <HDF5 dataset "acce": shape (64752, 3), type "<f8">
gyro = gyro_uncalib - np.array(self.info['imu_init_gyro_bias']) # array([[-0.02230625, 0.01376489, 0.01876768], [-0.01673078, 0.00785385, 0.01808999], ..., [ 0.00657855, -0.02807542, 0.00804713]])
acce = np.array(self.info['imu_acce_scale']) * (acce_uncalib - np.array(self.info['imu_acce_bias'])) # array([[-9.76895341, -0.19332236, -0.85234999], [-9.76140265, -0.2099069 , -0.81018915], ..., [-9.82066284, -0.32593967, -0.28265888]])
ts = np.copy(f['synced/time']) # array([3641.39639807, 3641.40139807, 3641.40639807, ..., 3965.14139807, 3965.14639807, 3965.15139807])
tango_pos = np.copy(f['pose/tango_pos']) # array([[ 0.00747055, 0.0794231 , 0.04721916], [ 0.00743938, 0.07954534, 0.04721213], ..., [ 0.04869788, -0.01891041, -0.03532039]])
init_tango_ori = quaternion.quaternion(*f['pose/tango_ori'][0]) # quaternion(0.500218919180744, 0.498520940104168, 0.458115146317795, -0.539803994906776)
# Compute the IMU orientation in the Tango coordinate frame.
ori_q = quaternion.from_float_array(ori) # array([quaternion(0.0230138445473811, -0.734161004581412, 0.00956858773770847, 0.678517142961637), quaternion(0.0229602307881793, -0.734172007659053, 0.00956628356173319, 0.678507709011024), ..., quaternion(0.0542799190079345, 0.708817623072373, 0.0763779734707989, -0.6991362963311)], dtype=quaternion)
# hdf5.synced.game_rv i. e. Android Sensor.TYPE_GAME_ROTATION_VECTOR
rot_imu_to_tango = quaternion.quaternion(*self.info['start_calibration']) # quaternion(0, 0.99995545, 0.009439, 0)
init_rotor = init_tango_ori * rot_imu_to_tango * ori_q[0].conj() # quaternion(-0.695289552060529, 0.00118374652078425, 0.00248606386725569, 0.718723783950829)
ori_q = init_rotor * ori_q # array([quaternion(-0.5028224217168, 0.50529138394065, -0.535057892743795, -0.453388785030156), quaternion(-0.502778345472314, 0.505300603413145, -0.535064320967734, -0.453420734559951), ..., quaternion(0.463716682908265, -0.54940199756135, 0.457301820727505, 0.523442677366401)], dtype=quaternion)
# ori_q = f['pose/tango_ori'][0] * self.info['start_calibration'] * conj(f['synced/game_rv'][0]) * f['synced/game_rv']
dt = (ts[self.w:] - ts[:-self.w])[:, None] # array([[1.], [1.], [1.], ..., [1.],[1.], [1.]])
glob_v = (tango_pos[self.w:] - tango_pos[:-self.w]) / dt # array([[-0.00533056, 0.01667982, -0.00509732], [-0.00531125, 0.016594 , -0.00511179], ..., [-0.0023489 , 0.00633583, -0.00057166]])
# these two below are position vector arrays
gyro_q = quaternion.from_float_array(np.concatenate([np.zeros([gyro.shape[0], 1]), gyro], axis=1)) # array([quaternion(0, -0.0223062507369463, 0.0137648946088728, 0.0187676819234896), quaternion(0, -0.0167307794589504, 0.00785385399152264, 0.018089989505323), ..., quaternion(0, 0.00657855020053501, -0.0280754170707831, 0.0080471337151681)], dtype=quaternion)
acce_q = quaternion.from_float_array(np.concatenate([np.zeros([acce.shape[0], 1]), acce], axis=1)) # array([quaternion(0, -9.76895340810378, -0.193322357928738, -0.852349993053583), quaternion(0, -9.76140265037272, -0.209906902110648, -0.810189151712629), ..., quaternion(0, -9.8206628384554, -0.325939671417927, -0.282658875290474)], dtype=quaternion)
# each element vector rotated by the corresponding ori_q rotation quaternion
# At test time, we use the coordinate frame defined by system device orientations from Android or iOS, whose Z axis is aligned with gravity.
# The whole is transformed into the global (Tango) coordinate frame.
glob_gyro = quaternion.as_float_array(ori_q * gyro_q * ori_q.conj())[:, 1:] # array([[-0.01258328, 0.02161022, 0.02034508], [-0.00665554, 0.02000185, 0.0149826 ], ..., [ 0.02674353, 0.01209917, -0.0058859 ]])
glob_acce = quaternion.as_float_array(ori_q * acce_q * ori_q.conj())[:, 1:] # array([[-3.46650073e-02, -3.36474233e-02, 9.80783499e+00], [-1.38849800e-02, 6.54256497e-03, 9.79719413e+00], ..., [ 3.31727211e-02, -6.26925428e-02, 9.82980926e+00]])
start_frame = self.info.get('start_frame', 0) # 5896
self.ts = ts[start_frame:] # array([3670.87639807, 3670.88139807, 3670.88639807, ..., 3965.14139807, 3965.14639807, 3965.15139807])
self.features = np.concatenate([glob_gyro, glob_acce], axis=1)[start_frame:] # array([[-5.94662071e-03, -9.38751552e-03, -5.15188486e-03, -3.08588928e-02, 6.39869105e-02, 9.86019268e+00], [-5.27530580e-03, -7.75847573e-03, -1.59778536e-02, -3.54599110e-02, 5.16253587e-02, 9.82394159e+00], ..., [ 2.67435308e-02, 1.20991655e-02, -5.88589595e-03, 3.31727211e-02, -6.26925428e-02, 9.82980926e+00]])
self.targets = glob_v[start_frame:, :2] # array([[-0.02427537, 0.02117807], [-0.02406481, 0.02145767], ..., [-0.0023489 , 0.00633583]]) targets are averaged for the window w
self.orientations = quaternion.as_float_array(ori_q)[start_frame:] # array([[-0.51946022, 0.24279321, -0.60182678, 0.55589147], [-0.51947897, 0.24272502, -0.6018242 , 0.55590699], ..., [ 0.46371668, -0.549402 , 0.45730182, 0.52344268]])
self.gt_pos = tango_pos[start_frame:] # array([[ 0.17387274, -0.14344794, -0.0743621 ], [ 0.17362087, -0.14350179, -0.07425673], ..., [ 0.04869788, -0.01891041, -0.03532039]])
def r_with_reduced_weights(q, t, weights_not_normd, xp, yp, hdx_R, hdy_R,
hnd_raw_R):
q = quaternion.as_float_array(q)
t = quaternion.as_float_array(t)[1:]
a = 2 * np.arccos(q[0])
if a != 0:
u = q[1:] / np.sin(a / 2)
else:
u = np.array([0, 0, 0])
angle_mat = (np.cos(a) - 1) * np.einsum('i,j->ij', u, u)\
+ np.sin(a) * np.einsum('ijk,k->ij', np.array([[[LeviCivita(i, j, k) for k in range(3)] for j in range(3)] for i in range(3)], dtype=np.double), u)\
- np.cos(a) * np.eye(3)
hnd_R = 2 * np.einsum('ijkl,kl->ij', hnd_raw_R, angle_mat)
def load(self, data_path):
if data_path[-1] == '/':
data_path = data_path[:-1]
with open(osp.join(data_path, 'info.json')) as f:
self.info = json.load(f)
self.info['path'] = osp.split(data_path)[-1]
if self.with_orientation:
self.info['ori_source'], ori, self.info['source_ori_error'] = select_orientation_source(
data_path, self.max_ori_error, self.grv_only)
with h5py.File(osp.join(data_path, 'data.hdf5')) as f:
gyro_uncalib = f['synced/gyro_uncalib']
acce_uncalib = f['synced/acce']
gyro = gyro_uncalib - np.array(self.info['imu_init_gyro_bias'])
# acce = np.array(self.info['imu_acce_scale']) * (acce_uncalib - np.array(self.info['imu_acce_bias']))
acce = acce_uncalib
ts = np.copy(f['synced/time'])
# tango_pos = np.copy(f['pose/tango_pos'])
# init_tango_ori = quaternion.quaternion(*f['pose/tango_ori'][0])
if self.with_orientation:
gyro = np.copy(gyro)
acce = np.copy(acce)
else:
glob_gyro = np.copy(gyro)
glob_acce = np.copy(acce)
# Compute the IMU orientation in the Tango coordinate frame.
if self.with_orientation:
ori_q = quaternion.from_float_array(ori)
# rot_imu_to_tango = quaternion.quaternion(*self.info['start_calibration'])
# init_rotor = init_tango_ori * rot_imu_to_tango * ori_q[0].conj()
# ori_q = init_rotor * ori_q
gyro_q = quaternion.from_float_array(np.concatenate([np.zeros([gyro.shape[0], 1]), gyro], axis=1))
acce_q = quaternion.from_float_array(np.concatenate([np.zeros([acce.shape[0], 1]), acce], axis=1))
glob_gyro = quaternion.as_float_array(ori_q * gyro_q * ori_q.conj())[:, 1:]
glob_acce = quaternion.as_float_array(ori_q * acce_q * ori_q.conj())[:, 1:]
# dt = (ts[self.w:] - ts[:-self.w])[:, None]
# glob_v = (tango_pos[self.w:] - tango_pos[:-self.w]) / dt
start_frame = self.info.get('start_frame', 0)
self.ts = ts[start_frame:]
self.features = np.concatenate([glob_gyro, glob_acce], axis=1)[start_frame:]
def fast_findanalytic_BT_newton(x,
y,
xp,
yp,
q,
weights,
r_y,
final_run=False):
y = quaternion.as_float_array(y)[:, 1:]
H = np.zeros((6, 6))
h_bb = 8 * np.einsum('ij,mj,kl->imkl', x, y, np.eye(3)) - 4 * \
np.einsum('ij,mk->imjk', x, y)-4 * np.einsum('ij,mk->imkj', x, y)
H[:3, :3] = np.einsum('ij,ijkl->kl', weights, h_bb)
h_bt = 4 * np.einsum(
'ij,klj->ikl', y,
np.array([[[LeviCivita(i, j, k) for k in range(3)] for j in range(3)]
for i in range(3)],
dtype=np.double))
H[:3, 3:] = np.einsum('ij,jkl->kl', weights, h_bt)
H[3:, 3:] = 2 * np.eye(3) * np.sum(weights)
H[3:, :3] = np.transpose(H[:3, 3:])
L = np.zeros(6)
l = np.zeros((len(xp), len(xp), 6))
l[:, :, :3] = 4 * np.einsum(
'ik,jl,mkl->ijm', x, y,
np.array([[[LeviCivita(i, j, k) for k in range(3)] for j in range(3)]
for i in range(3)],
dtype=np.double))
l[:, :,
3:] = 2 * (np.reshape(np.hstack(len(x) * [x]), (len(x), len(x), 3)) -
np.reshape(np.vstack(len(y) * [y]), (len(y), len(y), 3)))
L = np.einsum('ij,ijk->k', weights, l)
if final_run:
dLdrx = np.zeros((len(x), 6))
dLdrx[:, :3] = 4 * np.einsum(
'ij,ik,jl,mkl->im', weights, xp, y,
np.array([[[LeviCivita(i, j, k) for k in range(3)]
for j in range(3)] for i in range(3)],
dtype=np.double))
dLdrx[:, 3:] = 2 * np.einsum('ij,ik->ik', weights, xp)
ytilde = quaternion.as_float_array(
[q * np.quaternion(*yi) * np.conjugate(q) for yi in yp])[:, 1:]
dLdry = np.zeros((len(y), 6))
dLdry[:, :3] = 4 * np.einsum(
'ij,ik,jl,mkl->jm', weights, x, ytilde,
np.array([[[LeviCivita(i, j, k) for k in range(3)]
for j in range(3)] for i in range(3)],
dtype=np.double))
dLdry[:, 3:] = -2 * np.einsum('ij,jk->jk', weights, ytilde)
return -np.linalg.inv(H) @ L, l, dLdrx, dLdry, np.linalg.inv(H)
return -np.linalg.inv(H) @ L
def pow(self, exponent):
"""self^exponent
:param exponent: single float
"""
exponent = float(exponent)
q_r_float = quaternion.as_float_array(self.q_r)
q_d_float = quaternion.as_float_array(self.q_d)
if len(self)==1:
q_r_w, q_r_vec = self.q_r.w, self.q_r.vec
q_d_w, q_d_vec = self.q_d.w, self.q_d.vec
q_r_w, q_r_vec = np.array([q_r_w])[:,None], np.array([q_r_vec])[:,None]
q_d_w, q_d_vec = np.array([q_d_w])[:, None], np.array([q_d_vec])[:, None]
else:
q_r_float = quaternion.as_float_array(self.q_r)
q_d_float = quaternion.as_float_array(self.q_d)
q_r_w = q_r_float[:,0]
q_r_vec = q_r_float[:,1:]
q_d_w = q_d_float[:, 0]
q_d_vec = q_d_float[:,1:]
theta = 2 * np.arccos(q_r_w)
if np.all(np.isclose(theta, 0)):
return DualQuaternion.from_translation_vector(exponent * np.array(self.translation()))
else:
s0 = q_r_vec / np.sin(theta[:,None] / 2)
d = -2. * q_d_w / np.sin(theta / 2)
se = (q_d_vec - s0 * d[:,None] / 2 * np.cos(theta[:,None] / 2)) / np.sin(theta[:,None] / 2)
q_r_float = np.zeros((len(self),4))
q_r_float[:,0] = np.cos(exponent * theta / 2)
q_r_float[:,1:] = np.sin(exponent * theta[:,None] / 2) * s0
q_r = quaternion.from_float_array(q_r_float)
q_d_float = np.zeros((len(self), 4))
q_d_float[:,0] = -exponent * d / 2 * np.sin(exponent * theta / 2)
#q_d_float[:,1:] = exponent * d / 2 * np.cos(exponent * theta / 2) * s0 + np.sin(exponent * theta / 2) * se
p3 = (exponent * d /2 *np.cos(exponent * theta / 2))[:,None] * s0 + np.sin(exponent * theta[:,None] / 2) * se
q_d_float[:,1:] = p3
q_d = quaternion.from_float_array(q_d_float)
return DualQuaternion(q_r, q_d)
def test_canonicals_quaternion(self):
expect_r = quaternion.from_float_array([1., 0., 0., 0.])
expect_t = np.array([0., 0., 0.])
pose = kapture.PoseTransform(expect_r, expect_t)
actual_r = pose.r
actual_t = pose.t
# check r
self.assertIsInstance(actual_r, np.quaternion)
self.assertListEqual(quaternion.as_float_array(
actual_r).tolist(), quaternion.as_float_array(expect_r).tolist())
# check t
self.assertIsInstance(actual_t, np.ndarray)
self.assertTupleEqual(actual_t.shape, (3, 1))
self.assertListEqual(actual_t.flatten().tolist(), expect_t.tolist())
def dq_array(self):
"""
Get the float array version of the dual quaternion as the rotation quaternion followed by the translation quaternion
:return: float array [q_rw, q_rx, q_ry, q_rz, q_tx, q_ty, q_tz]
"""
if isinstance(self.q_r,np.quaternion):
return np.array([self.q_r.w,self.q_r.x,self.q_r.y,self.q_r.z,self.q_d.w,self.q_r.x,self.q_r.y,self.q_r.z])
elif isinstance(self.q_r,np.ndarray):
if len(self)==1:
dq_floats = np.concatenate([quaternion.as_float_array(self.q_r[0]),quaternion.as_float_array(self.q_d[0])],axis=0)
else:
dq_floats = np.concatenate([quaternion.as_float_array(self.q_r), quaternion.as_float_array(self.q_d)],axis=1)
return dq_floats
def _predict(self, base, gyro, dt):
#
# Eq 40.
w_x, w_y, w_z = gyro
sub_array = np.array([[0, -w_z, w_y], [w_z, 0, -w_x], [-w_y, w_x, 0]])
omega = np.zeros((4,4))
omega[0, 1:4] = gyro.T
omega[1:4, 0] = -gyro
omega[1:4, 1:4] = -sub_array
#
# Eq 42.
npret = quaternion.as_float_array(base) + np.dot(omega, quaternion.as_float_array(base)) * dt * 0.5
#
# Weird thing to allow for constructor to work with a numpy array.
return np.quaternion(*npret).normalized()
def init_BT(zahl):
x = []
for _ in range(zahl):
x.append(
np.quaternion(0, random.random(), random.random(),
random.random()))
t = 0.1 * np.quaternion(0, random.random(), random.random(),
random.random())
b = .1 * np.quaternion(0, random.random(), random.random(),
random.random())
q = np.exp(b)
y = np.array([np.conjugate(q) * (xi + t) * q for xi in x])
weights = np.eye(zahl)
return quaternion.as_float_array(x), quaternion.as_float_array(
y), q, t, weights
def angular_velocity(self, frame=None):
"""Angular velocity of Waveform
This function calculates the angular velocity of a Waveform object from
its modes. This was introduced in Sec. II of "Angular velocity of
gravitational radiation and the corotating frame"
<http://arxiv.org/abs/1302.2919>. Essentially, this is the angular
velocity of the rotating frame in which the time dependence of the modes
is minimized.
The vector is given in the (possibly rotating) mode frame (X,Y,Z),
which is not necessarily equal to the inertial frame (x,y,z).
Parameters
----------
frame : array of quaternions (optional)
If provided, the velocity of the frame will be included in determining
the angular velocity of the waveform.
Returns
-------
numpy.ndarray
"""
from .mode_calculations import _LdtVector, _LLMatrix
# Calculate the <Ldt> vector at each instant
l = np.zeros((self.n_times, 3), dtype=float)
_LdtVector(self.ndarray, self.dot.ndarray, self.LM, l)
# Calculate the <LL> matrix at each instant
ll = np.zeros((self.n_times, 3, 3), dtype=float)
_LLMatrix(self.ndarray, self.LM, ll)
# Solve <Ldt> = - <LL> . omega
omega = -np.linalg.solve(ll, l)
if not frame is None:
if len(frame) != self.n_times:
raise ValueError(
f"len(frame)={len(frame)} does not match n_times={self.n_times}."
)
from quaternion import derivative, as_quat_array, as_float_array
Rdot = as_quat_array(derivative(as_float_array(frame), self.t))
omega += as_float_array(2 * Rdot * frame.conjugate())[:, 1:]
return omega
def test_numpy_array_conversion(Qs):
"Check conversions between array as quaternions and array as floats"
# First, just check 1-d array
Q = Qs[Qs_nonnan][:12] # Select first 3x4=12 non-nan elements in Qs
assert Q.dtype == np.dtype(np.dual_quaternion)
q = quaternion.as_float_array(Q) # View as array of floats
assert q.dtype == np.dtype(np.float)
assert q.shape == (12, 8) # This is the expected shape
for j in range(12):
for k in range(4): # Check each component individually
assert q[j][k] == Q[j].components[k]
assert np.array_equal(quaternion.as_dual_quat_array(q),
Q) # Check that we can go backwards
# Next, see how that works if I flatten the q array
q = q.flatten()
assert q.dtype == np.dtype(np.float)
assert q.shape == (96, )
for j in range(96):
assert q[j] == Q[j // 8].components[j % 8]
assert np.array_equal(quaternion.as_dual_quat_array(q),
Q) # Check that we can go backwards
# Now, reshape into 2-d array, and re-check
P = Q.reshape(3, 4) # Reshape into 3x4 array of quaternions
p = quaternion.as_float_array(P) # View as array of floats
assert p.shape == (3, 4, 8) # This is the expected shape
for j in range(3):
for k in range(4):
for l in range(8): # Check each component individually
assert p[j][k][l] == Q[4 * j + k].components[l]
assert np.array_equal(quaternion.as_dual_quat_array(p),
P) # Check that we can go backwards
# Check that we get an exception if the final dimension is not divisible by 8
with pytest.raises(ValueError):
quaternion.as_dual_quat_array(np.random.rand(4, 1))
with pytest.raises(ValueError):
quaternion.as_dual_quat_array(np.random.rand(4, 2))
with pytest.raises(ValueError):
quaternion.as_dual_quat_array(np.random.rand(4, 14))
with pytest.raises(ValueError):
quaternion.as_dual_quat_array(np.random.rand(4, 17))
with pytest.raises(ValueError):
quaternion.as_dual_quat_array(np.random.rand(4, 5, 3, 2, 1))
# Finally, check that it works on non-contiguous arrays, by adding random padding and then slicing
q = quaternion.as_float_array(Q)
q = np.concatenate(
(np.random.rand(q.shape[0], 3), q, np.random.rand(q.shape[0], 3)),
axis=1)
assert np.array_equal(quaternion.as_dual_quat_array(q[:, 3:11]), Q)
def SWSH(R, s, indices):
"""Spin-weighted spherical harmonic calculation from rotor
Note that this function is more general than standard Ylm and sYlm functions of angles because it uses quaternion
rotors instead of angle, and is slightly faster as a result.
This function can be called in either of two ways:
1) With an array of quaternions, and a single (ell,m) pair, or
2) with a single quaternion, and an array of (ell,m) values
Parameters
----------
R : unit quaternion or array of unit quaternions
Rotor on which to evaluate the SWSH function.
s : int
Spin weight of the field to evaluate
indices : 2-d array of int or pair of ints
Array of (ell,m) values to evaluate
Returns
-------
array of complex
The shape of this array is `indices.shape[0]`, and contains the values of the SWSH for the (ell,m) values
specified in `indices`.
"""
indices = np.array(indices)
if indices.size > 2 or not isinstance(R, np.ndarray):
values = np.empty((indices.shape[0],), dtype=complex)
_SWSH(R.a, R.b, s, indices, values)
else:
values = np.empty((R.size,), dtype=complex)
_SWSHs(quaternion.as_float_array(R.flatten()), s, indices[0], indices[1], values)
values = values.reshape(R.shape)
return values
def test_as_float_quat(Qs):
qs = Qs[Qs_nonnan]
for quats in [qs, np.vstack((qs,)*3), np.vstack((qs,)*(3*5)).reshape((3, 5)+qs.shape),
np.vstack((qs,)*(3*5*6)).reshape((3, 5, 6)+qs.shape)]:
floats = quaternion.as_float_array(quats)
assert floats.shape == quats.shape+(4,)
assert allclose(quaternion.as_quat_array(floats), quats)
def sclerp(cls, start, stop, t):
"""Screw Linear Interpolation
Generalization of Quaternion slerp (Shoemake et al.) for rigid body motions
ScLERP guarantees both shortest path (on the manifold) and constant speed
interpolation and is independent of the choice of coordinate system.
ScLERP(dq1, dq2, t) = dq1 * dq12^t where dq12 = dq1^-1 * dq2
:param start: DualQuaternion instance
:param stop: DualQuaternion instance
:param t: fraction betweem [0, 1] representing how far along and around the
screw axis to interpolate
"""
# ensure we always find closest solution. See Kavan and Zara 2005
mult = start.q_r * stop.q_r
if not isinstance(mult,np.ndarray):
mult = np.array([mult])
if len(start)>1:
mult_w = quaternion.as_float_array(mult)[:,0]
if np.any(mult_w<0):
start.q_r[np.where(mult_w<0)]*=-1
else:
mult_w = mult[0].w
if mult_w < 0:
start.q_r *= -1
return start * (start.inverse() * stop).pow(t)
def test_dpa_rotated_generally(w, Rs):
from scri.mode_calculations import LLDominantEigenvector
n_copies = 10
W = w(begin=1.0,
end=100.0,
n_times=n_copies * len(Rs),
ell_min=0,
ell_max=8)
assert W.ensure_validity(alter=False)
R_basis = np.array([R for R in Rs for i in range(n_copies)])
# We use `begin=1.0` because we need to avoid situations where the modes
# are all zeros, which can happen in `linear_waveform` at t=0.0
LL1 = LLDominantEigenvector(W)
LL1 = quaternion.as_float_array(
np.array([
R * np.quaternion(0, *v) * (~R) for R, v in zip(R_basis, LL1)
]))[:, 1:]
W.rotate_physical_system(R_basis)
LL2 = LLDominantEigenvector(W)
# Because the dpa is only defined up to a sign, all we need is for the
# dot product between the dpa and the expected value to be close to
# either 1 or -1. This finds the largest difference, based on the
# smaller of the two sign options.
assert (max(
np.amin(np.vstack((np.linalg.norm(
LL1 - LL2, axis=1), np.linalg.norm(LL1 + LL2, axis=1))),
axis=0)) < 1.0e-12)
def q_to_ypr(q):
# from https://math.stackexchange.com/questions/687964/getting-euler-tait-bryan-angles-from-quaternion-representation
q0,q1,q2,q3 = quaternion.as_float_array(q)
roll = np.arctan2(q2*q3+q0*q1, .5-q1**2-q2**2)
lat = -np.arcsin(-2*(q1*q3-q0*q2))
lon = np.arctan2(q1*q2+q0*q3, .5-q2**2-q3**2)
return lat, lon, roll
def angular_velocity(R, t):
from scipy.interpolate import InterpolatedUnivariateSpline as spline
R = quaternion.as_float_array(R)
Rdot = np.empty_like(R)
for i in range(4):
Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
return np.array([omega.vec for omega in (2*Rdot/R)])
def minimal_rotation(R, t, iterations=2):
"""Adjust frame so that there is no rotation about z' axis
The output of this function is a frame that rotates the z axis onto the same z' axis as the
input frame, but with minimal rotation about that axis. This is done by pre-composing the input
rotation with a rotation about the z axis through an angle gamma, where
dgamma/dt = 2*(dR/dt * z * R.conjugate()).w
This ensures that the angular velocity has no component along the z' axis.
Note that this condition becomes easier to impose the closer the input rotation is to a
minimally rotating frame, which means that repeated application of this function improves its
accuracy. By default, this function is iterated twice, though a few more iterations may be
called for.
Parameters
==========
R: quaternion array
Time series describing rotation
t: float array
Corresponding times at which R is measured
iterations: int [defaults to 2]
Repeat the minimization to refine the result
"""
from scipy.interpolate import InterpolatedUnivariateSpline as spline
if iterations == 0:
return R
R = quaternion.as_float_array(R)
Rdot = np.empty_like(R)
for i in range(4):
Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
halfgammadot = quaternion.as_float_array(Rdot * quaternion.z * R.conjugate())[:, 0]
halfgamma = spline(t, halfgammadot).antiderivative()(t)
Rgamma = np.exp(quaternion.z * halfgamma)
return minimal_rotation(R * Rgamma, t, iterations=iterations-1)
def quat_mat_vec(quats):
mat_vec = np.array([quaternion.as_float_array(quats * v * np.invert(quats))[..., 1:]
for v in [quaternion.x, quaternion.y, quaternion.z]])
return np.transpose(mat_vec, tuple(range(mat_vec.ndim))[1:-1]+(-1, 0))
