def get_rot(self): return matrix_from_quat(self.quat)
def __init__(self, quat=quat_identity(), ang=np.zeros(3), in_body=False): self.rot = matrix_from_quat(quat) self.ang = ang.copy() self.in_body = in_body
aa = ang_acc * aa_base
t1 = time.process_time()
body_rot.step(dt, ang_accel=aa)
t2 = time.process_time()
body_quat.step(dt, ang_accel=aa)
t3 = time.process_time()
body_quat_nolie.step(dt, ang_accel=aa)
t4 = time.process_time()
rot_ts.append(t2 - t1)
quat_ts.append(t3 - t2)
quatnolie_ts.append(t4 - t3)
res2 = matrix_from_quat(body_quat.quat)
res3 = body_rot.rot
data = [
("Quat + Norm", matrix_from_quat(body_quat_nolie.quat), quatnolie_ts),
("Quat w/ Exp. Map", matrix_from_quat(body_quat.quat), quat_ts),
("SO(3) w/ Exp. Map", body_rot.rot, rot_ts)
]
print("N = %d, dt = %f" % (N, dt))
for s, rot, _ in data:
print("%s (%s)" % (s, ["WRONG - did not return to origin", "CORRECT"][np.allclose(rot, np.eye(3))]))
print('\tEuler angles: %s' % str(R.from_matrix(rot).as_euler('ZYX')))
print("Avg time per step (us)")
for s, _, ts in data:
print("\t%s: %f (%f std)" % (s, 1e6 * np.mean(ts), 1e6 * np.std(ts)))