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)