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