def test_dual_quaternion_sclerp(): random_state = np.random.RandomState(22) pose1 = random_transform(random_state) pose2 = random_transform(random_state) dq1 = dual_quaternion_from_transform(pose1) dq2 = dual_quaternion_from_transform(pose2) n_steps = 100 # Ground truth: screw linear interpolation pose12pose2 = concat(pose2, invert_transform(pose1)) screw_axis, theta = screw_axis_from_exponential_coordinates( exponential_coordinates_from_transform(pose12pose2)) offsets = np.array([ transform_from_exponential_coordinates(screw_axis * t * theta) for t in np.linspace(0, 1, n_steps) ]) interpolated_poses = np.array( [concat(offset, pose1) for offset in offsets]) # Dual quaternion ScLERP sclerp_interpolated_dqs = np.vstack([ dual_quaternion_sclerp(dq1, dq2, t) for t in np.linspace(0, 1, n_steps) ]) sclerp_interpolated_poses_from_dqs = np.array( [transform_from_dual_quaternion(dq) for dq in sclerp_interpolated_dqs]) for t in range(n_steps): assert_array_almost_equal(interpolated_poses[t], sclerp_interpolated_poses_from_dqs[t])
def test_invert_transform_without_check(): """Test inversion of transformations.""" random_state = np.random.RandomState(0) for _ in range(5): A2B = random_state.randn(4, 4) A2B = A2B + A2B.T B2A = invert_transform(A2B, check=False) A2B2 = np.linalg.inv(B2A) assert_array_almost_equal(A2B, A2B2)
def test_invert_transform(): """Test inversion of transformations.""" random_state = np.random.RandomState(0) for _ in range(5): R = matrix_from(a=random_axis_angle(random_state)) p = random_vector(random_state) A2B = transform_from(R, p) B2A = invert_transform(A2B) A2B2 = np.linalg.inv(B2A) assert_array_almost_equal(A2B, A2B2)
def test_request_concatenated_transform(): """Request a concatenated transform from the transform manager.""" random_state = np.random.RandomState(0) A2B = random_transform(random_state) B2C = random_transform(random_state) F2A = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "B", A2B) tm.add_transform("B", "C", B2C) tm.add_transform("D", "E", np.eye(4)) tm.add_transform("F", "A", F2A) A2C = tm.get_transform("A", "C") assert_array_almost_equal(A2C, concat(A2B, B2C)) C2A = tm.get_transform("C", "A") assert_array_almost_equal( C2A, concat(invert_transform(B2C), invert_transform(A2B))) F2B = tm.get_transform("F", "B") assert_array_almost_equal(F2B, concat(F2A, A2B))
def test_request_inverse_transform(): """Request an inverse transform from the transform manager.""" random_state = np.random.RandomState(0) A2B = random_transform(random_state) tm = TransformManager() tm.add_transform("A", "B", A2B) A2B_2 = tm.get_transform("A", "B") assert_array_almost_equal(A2B, A2B_2) B2A = tm.get_transform("B", "A") B2A_2 = invert_transform(A2B) assert_array_almost_equal(B2A, B2A_2)
def test_concat(): """Test concatenation of transforms.""" random_state = np.random.RandomState(0) for _ in range(5): A2B = random_transform(random_state) assert_transform(A2B) B2C = random_transform(random_state) assert_transform(B2C) A2C = concat(A2B, B2C) assert_transform(A2C) p_A = np.array([0.3, -0.2, 0.9, 1.0]) p_C = transform(A2C, p_A) C2A = invert_transform(A2C) p_A2 = transform(C2A, p_C) assert_array_almost_equal(p_A, p_A2) C2A2 = concat(invert_transform(B2C), invert_transform(A2B)) p_A3 = transform(C2A2, p_C) assert_array_almost_equal(p_A, p_A3)
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)
import pytransform3d.trajectories as ptr import pytransform3d.plot_utils as ppu random_state = np.random.RandomState(21) pose1 = pt.random_transform(random_state) pose2 = pt.random_transform(random_state) dq1 = pt.dual_quaternion_from_transform(pose1) dq2 = -pt.dual_quaternion_from_transform(pose2) Stheta1 = pt.exponential_coordinates_from_transform(pose1) Stheta2 = pt.exponential_coordinates_from_transform(pose2) n_steps = 100 # Ground truth screw motion: linear interpolation of rotation about and # translation along the screw axis pose12pose2 = pt.concat(pose2, pt.invert_transform(pose1)) screw_axis, theta = pt.screw_axis_from_exponential_coordinates( pt.exponential_coordinates_from_transform(pose12pose2)) offsets = np.array([ pt.transform_from_exponential_coordinates(screw_axis * t * theta) for t in np.linspace(0, 1, n_steps) ]) interpolated_poses = np.array([pt.concat(offset, pose1) for offset in offsets]) # Linear interpolation of dual quaternions interpolated_dqs = (np.linspace(1, 0, n_steps)[:, np.newaxis] * dq1 + np.linspace(0, 1, n_steps)[:, np.newaxis] * dq2) # renormalization (not required here because it will be done with conversion) interpolated_dqs /= np.linalg.norm(interpolated_dqs[:, :4], axis=1)[:, np.newaxis] interpolated_poses_from_dqs = np.array(