def test_check_transform_log():
assert_raises_regexp(
ValueError, "Expected array-like with shape", check_transform_log,
np.zeros((1, 4, 4)))
assert_raises_regexp(
ValueError, "Expected array-like with shape", check_transform_log,
np.zeros((3, 4)))
assert_raises_regexp(
ValueError, "Expected array-like with shape", check_transform_log,
np.zeros((4, 3)))
assert_raises_regexp(
ValueError, "Last row of logarithm of transformation must only "
"contains zeros",
check_transform_log, np.eye(4))
transform_log = screw_matrix_from_screw_axis(
np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0])) * 1.1
transform_log2 = check_transform_log(transform_log)
assert_array_almost_equal(transform_log, transform_log2)
transform_log = screw_matrix_from_screw_axis(
np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0])) * 1.1
transform_log[0, 0] = 0.0001
assert_raises_regexp(
ValueError, "Expected skew-symmetric matrix",
check_transform_log, transform_log)
def test_conversions_between_screw_matrix_and_transform_log():
S_mat = np.array([[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]])
theta = 1.0
transform_log = transform_log_from_screw_matrix(S_mat, theta)
S_mat2, theta2 = screw_matrix_from_transform_log(transform_log)
assert_array_almost_equal(S_mat, S_mat2)
assert_almost_equal(theta, theta2)
S_mat = np.zeros((4, 4))
theta = 0.0
transform_log = transform_log_from_screw_matrix(S_mat, theta)
S_mat2, theta2 = screw_matrix_from_transform_log(transform_log)
assert_array_almost_equal(S_mat, S_mat2)
assert_almost_equal(theta, theta2)
random_state = np.random.RandomState(65)
for _ in range(5):
S = random_screw_axis(random_state)
theta = np.random.rand()
S_mat = screw_matrix_from_screw_axis(S)
transform_log = transform_log_from_screw_matrix(S_mat, theta)
S_mat2, theta2 = screw_matrix_from_transform_log(transform_log)
assert_array_almost_equal(S_mat, S_mat2)
assert_almost_equal(theta, theta2)
def test_conversions_between_screw_matrix_and_screw_axis():
random_state = np.random.RandomState(83)
for _ in range(5):
S = random_screw_axis(random_state)
S_mat = screw_matrix_from_screw_axis(S)
S2 = screw_axis_from_screw_matrix(S_mat)
assert_array_almost_equal(S, S2)
def test_check_screw_matrix():
assert_raises_regexp(
ValueError, "Expected array-like with shape", check_screw_matrix,
np.zeros((1, 4, 4)))
assert_raises_regexp(
ValueError, "Expected array-like with shape", check_screw_matrix,
np.zeros((3, 4)))
assert_raises_regexp(
ValueError, "Expected array-like with shape", check_screw_matrix,
np.zeros((4, 3)))
assert_raises_regexp(
ValueError, "Last row of screw matrix must only contains zeros",
check_screw_matrix, np.eye(4))
screw_matrix = screw_matrix_from_screw_axis(
np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0])) * 1.1
assert_raises_regexp(
ValueError, "Norm of rotation axis must either be 0 or 1",
check_screw_matrix, screw_matrix)
screw_matrix = screw_matrix_from_screw_axis(
np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0])) * 1.1
assert_raises_regexp(
ValueError, "If the norm of the rotation axis is 0",
check_screw_matrix, screw_matrix)
screw_matrix = screw_matrix_from_screw_axis(
np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0]))
screw_matrix2 = check_screw_matrix(screw_matrix)
assert_array_almost_equal(screw_matrix, screw_matrix2)
screw_matrix = screw_matrix_from_screw_axis(
np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]))
screw_matrix2 = check_screw_matrix(screw_matrix)
assert_array_almost_equal(screw_matrix, screw_matrix2)
screw_matrix = screw_matrix_from_screw_axis(
np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0]))
screw_matrix[0, 0] = 0.0001
assert_raises_regexp(
ValueError, "Expected skew-symmetric matrix",
check_screw_matrix, screw_matrix)
def test_adjoint_of_transformation():
random_state = np.random.RandomState(94)
for _ in range(5):
A2B = random_transform(random_state)
theta_dot = 3.0 * random_state.rand()
S = random_screw_axis(random_state)
V_A = S * theta_dot
adj_A2B = adjoint_from_transform(A2B)
V_B = adj_A2B.dot(V_A)
S_mat = screw_matrix_from_screw_axis(S)
V_mat_A = S_mat * theta_dot
V_mat_B = A2B.dot(V_mat_A).dot(invert_transform(A2B))
S_B, theta_dot2 = screw_axis_from_exponential_coordinates(V_B)
V_mat_B2 = screw_matrix_from_screw_axis(S_B) * theta_dot2
assert_almost_equal(theta_dot, theta_dot2)
assert_array_almost_equal(V_mat_B, V_mat_B2)
