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_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)
def test_conversions_between_screw_axis_and_exponential_coordinates():
S = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0])
theta = 1.0
Stheta = exponential_coordinates_from_screw_axis(S, theta)
S2, theta2 = screw_axis_from_exponential_coordinates(Stheta)
assert_array_almost_equal(S, S2)
assert_almost_equal(theta, theta2)
S = np.zeros(6)
theta = 0.0
S2, theta2 = screw_axis_from_exponential_coordinates(np.zeros(6))
assert_array_almost_equal(S, S2)
assert_almost_equal(theta, theta2)
random_state = np.random.RandomState(33)
for _ in range(5):
S = random_screw_axis(random_state)
theta = random_state.rand()
Stheta = exponential_coordinates_from_screw_axis(S, theta)
S2, theta2 = screw_axis_from_exponential_coordinates(Stheta)
assert_array_almost_equal(S, S2)
assert_almost_equal(theta, theta2)
def test_random_screw_axis():
random_state = np.random.RandomState(893)
for _ in range(5):
S = random_screw_axis(random_state)
check_screw_axis(S)