def _check_state_expected(s1, s2, expected: ExpectedDelta): assert np.linalg.norm(s2.position - s1.position - expected.delta_pos) < 1e-5 assert (angle_between_quats(s2.rotation * expected.delta_rot.inverse(), s1.rotation) < 1e-5)
def _check_state_same(s1, s2): assert np.allclose(s1.position, s2.position) assert angle_between_quats(s1.rotation, s2.rotation) < 1e-5