def test_construct_rigid_transformation():
tiny = 0.0001
rot_m = mat.construct_rx_matrix(np.pi / 2)
vm.validate_rotation_matrix(rot_m)
trans_v = np.array([[10], [10], [10]])
vm.validate_translation_column_vector(trans_v)
rigid_transformation = mat.construct_rigid_transformation(rot_m, trans_v)
vm.validate_rigid_matrix(rigid_transformation)
assert np.abs(rigid_transformation[0][0] - 1) < tiny
assert np.abs(rigid_transformation[0][1]) < tiny
assert np.abs(rigid_transformation[0][2]) < tiny
assert np.abs(rigid_transformation[0][3] - 10) < tiny
assert np.abs(rigid_transformation[1][0]) < tiny
assert np.abs(rigid_transformation[1][1]) < tiny
assert np.abs(rigid_transformation[1][2] + 1) < tiny
assert np.abs(rigid_transformation[1][3] - 10) < tiny
assert np.abs(rigid_transformation[2][0]) < tiny
assert np.abs(rigid_transformation[2][1] - 1) < tiny
assert np.abs(rigid_transformation[2][2]) < tiny
assert np.abs(rigid_transformation[2][3] - 10) < tiny
assert np.abs(rigid_transformation[3][0]) < tiny
assert np.abs(rigid_transformation[3][1]) < tiny
assert np.abs(rigid_transformation[3][2]) < tiny
assert np.abs(rigid_transformation[3][3] - 1) < tiny
def check_construct_rz_matrix(angle, is_in_radians, point):
""""
Check if the rotation matrix for rotating around the z axis is correct.
:param angle: the angle to rotate
:param is_in_radians: if angle is in radians or not
:param point: the point to be rotated
:returns: new_point -- the point after rotation
"""
rot_z = mat.construct_rz_matrix(angle, is_in_radians)
vm.validate_rotation_matrix(rot_z)
new_point = np.matmul(rot_z, point)
assert new_point[2] == point[2]
assert np.abs(np.linalg.norm(new_point) - np.linalg.norm(point)) <= 0.0001
return new_point
def check_construct_rotm_from_euler(angle_a, angle_b, angle_c, sequence,
is_in_radians, point):
""""
Check if the rotation matrix for rotating around the x axis is correct.
:param angle_a: first Euler angle
:param angle_b: second Euler angle
:param angle_c: third Euler angle
:param is_in_radians: if angle is in radians or not
:param point: the point to be rotated
:returns: new_point -- the point after rotation
"""
rot_m = mat.construct_rotm_from_euler(angle_a, angle_b, angle_c, sequence,
is_in_radians)
vm.validate_rotation_matrix(rot_m)
new_point = np.matmul(rot_m, point)
assert np.abs(np.linalg.norm(new_point) - np.linalg.norm(point)) <= 0.0001
return new_point
def test_rotation_matrix_invalid_because_too_many_columns():
with pytest.raises(ValueError):
vm.validate_rotation_matrix(np.ones((3, 4)))
def test_rotation_matrix_invalid_because_too_few_rows():
with pytest.raises(ValueError):
vm.validate_rotation_matrix(np.ones((2, 3)))
def test_rotation_matrix_invalid_because_not_two_dimensional():
with pytest.raises(ValueError):
vm.validate_rotation_matrix(np.ones((3, 3, 3)))
def test_rotation_matrix_invalid_because_wrong_type():
with pytest.raises(TypeError):
vm.validate_rotation_matrix(1)
def test_rotation_matrix_valid():
rotation_matrix = np.eye(3)
assert vm.validate_rotation_matrix(rotation_matrix)
def test_rotation_matrix_invalid_because_determinant_not_positive():
with pytest.raises(ValueError):
vm.validate_rotation_matrix(
np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]))
def test_rotation_matrix_invalid_because_not_orthogonal():
with pytest.raises(ValueError):
vm.validate_rotation_matrix(
np.array([[3.0, -4.0, 1.0], [5.0, 3.0, -7.0], [-9.0, 2.0, 6.0]]))