def test_create_line(self): pos = [1, 2, 3] tol = [ SymmetricTolerance(1, 10), SymmetricTolerance(1, 10), SymmetricTolerance(1, 10), ] start_pt = TolPositionPt(pos, Quaternion.random(), tol) path = create_line(start_pt, np.array([2, 2, 3]), 3) assert path[-1].pos[0] == 2 assert_almost_equal(path[1].pos[0], 1.5) assert path[-1].pos[1] == pos[1] assert path[-1].pos[1] == pos[1] assert path[-1].pos[2] == pos[2] assert path[-1].pos[2] == pos[2] assert path[-1].pos[0] == pos[0] + 1 assert path[-1].pos[0] == pos[0] + 1
def test_max_distance(): for _ in range(10): dist = quat_distance(Quaternion.random(), Quaternion.random()) assert dist <= (0.5 * np.pi)
def test_matrix_to_rxyz(self): for _ in range(100): R_random = Quaternion.random().rotation_matrix rpy = rotation_matrix_to_rpy(R_random) R_converted = rpy_to_rot_mat(rpy) assert_almost_equal(R_random, R_converted)