import numpy as np
from tqdm import tqdm
import matplotlib.pyplot as plt
e = 1e-6
## Scratch remove axis of rotation from quaternion
u = np.array([[0, 0, 1.]]).T
# u = np.random.random((3,1))
u /= norm(u)
psi_hist, yaw_hist, s_hist, v_hist, qz_hist, qx_hist, qy_hist = [], [], [], [], [], [], []
for i in tqdm(range(10000)):
# qm0 = Quaternion.from_euler(0.0, 10.0, 2*np.pi*np.random.random() - np.pi)
# qm0 = Quaternion.from_euler(0.0, np.pi*np.random.random() - np.pi/2.0, 0.0)
qm0 = Quaternion.from_euler(2*np.pi*np.random.random() - np.pi, np.pi * np.random.random() - np.pi / 2.0, 0.0)
# qm0 = Quaternion.from_euler(270.0 * np.pi / 180.0, 85.0 * np.pi / 180.0, 90.0 * np.pi / 180.0)
# qm0 = Quaternion.random()
w = qm0.rot(u)
th = u.T.dot(w)
ve = skew(u).dot(w)
qp0 = Quaternion.exp(th * ve)
epsilon = np.eye(3) * e
t = u.T.dot(qm0.rot(u))
v = skew(u).dot(qm0.rot(u))
tv0 = u.T.dot(qm0.rot(u)) * (skew(u).dot(qm0.rot(u)))
a_dtvdq = -v.dot(u.T.dot(qm0.R.T).dot(skew(u))) - t*skew(u).dot(qm0.R.T.dot(skew(u)))
msg.angular_velocity.x = w[0, 0]
msg.angular_velocity.y = w[1, 0]
msg.angular_velocity.z = w[2, 0]
outbag.write('imu', msg, msg.header.stamp)
elif topic == '/vicon/firefly_sbx/firefly_sbx':
att = Quaternion(
np.array([[msg.transform.rotation.w
], [msg.transform.rotation.x],
[msg.transform.rotation.y],
[msg.transform.rotation.z]]))
pos = np.array([[msg.transform.translation.x],
[msg.transform.translation.y],
[msg.transform.translation.z]])
if q0 is None:
q0 = Quaternion.from_euler(0, 0, att.yaw)
p0 = pos.copy()
att = q0.inverse * att
pos = q0.invrot(pos - p0)
att_NED = q_NED_NWU.qinvrot(att)
pos_NED = q_NED_NWU.invrot(pos)
pose_msg = PoseStamped()
pose_msg.header = msg.header
pose_msg.pose.position.x = pos_NED[0, 0]
pose_msg.pose.position.y = pos_NED[1, 0]
pose_msg.pose.position.z = pos_NED[2, 0]
pose_msg.pose.orientation.w = att_NED.w
pose_msg.pose.orientation.x = att_NED.x