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)