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