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)