def test_conversions_between_exponential_coordinates_and_transform(): A2B = np.eye(4) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) A2B2 = transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B = translate_transform(np.eye(4), [1.0, 5.0, 0.0]) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0]) A2B2 = transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B = rotate_transform(np.eye(4), active_matrix_from_angle(2, 0.5 * np.pi)) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0]) A2B2 = transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) random_state = np.random.RandomState(52) for _ in range(5): A2B = random_transform(random_state) Stheta = exponential_coordinates_from_transform(A2B) A2B2 = transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2)
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_screw_parameters_from_dual_quaternion(): dq = np.array([1, 0, 0, 0, 0, 0, 0, 0]) q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq) assert_array_almost_equal(q, np.zeros(3)) assert_array_almost_equal(s_axis, np.array([1, 0, 0])) assert_true(np.isinf(h)) assert_almost_equal(theta, 0) dq = dual_quaternion_from_pq(np.array([1.2, 1.3, 1.4, 1, 0, 0, 0])) q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq) assert_array_almost_equal(q, np.zeros(3)) assert_array_almost_equal(s_axis, norm_vector(np.array([1.2, 1.3, 1.4]))) assert_true(np.isinf(h)) assert_almost_equal(theta, np.linalg.norm(np.array([1.2, 1.3, 1.4]))) random_state = np.random.RandomState(1001) quat = random_quaternion(random_state) a = axis_angle_from_quaternion(quat) dq = dual_quaternion_from_pq(np.r_[0, 0, 0, quat]) q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq) assert_array_almost_equal(q, np.zeros(3)) assert_array_almost_equal(s_axis, a[:3]) assert_array_almost_equal(h, 0) assert_array_almost_equal(theta, a[3]) for _ in range(5): A2B = random_transform(random_state) dq = dual_quaternion_from_transform(A2B) Stheta = exponential_coordinates_from_transform(A2B) S, theta = screw_axis_from_exponential_coordinates(Stheta) q, s_axis, h = screw_parameters_from_screw_axis(S) q2, s_axis2, h2, theta2 = screw_parameters_from_dual_quaternion(dq) assert_screw_parameters_equal(q, s_axis, h, theta, q2, s_axis2, h2, theta2)
def test_norm_exponential_coordinates(): Stheta_only_translation = np.array([0.0, 0.0, 0.0, 100.0, 25.0, -23.0]) Stheta_only_translation2 = norm_exponential_coordinates( Stheta_only_translation) assert_array_almost_equal(Stheta_only_translation, Stheta_only_translation2) random_state = np.random.RandomState(381) # 180 degree rotation ambiguity for _ in range(10): q = random_state.randn(3) s = norm_vector(random_state.randn(3)) h = random_state.randn() Stheta = screw_axis_from_screw_parameters(q, s, h) * np.pi Stheta2 = screw_axis_from_screw_parameters(q, -s, -h) * np.pi assert_array_almost_equal( transform_from_exponential_coordinates(Stheta), transform_from_exponential_coordinates(Stheta2)) assert_array_almost_equal(norm_exponential_coordinates(Stheta), norm_exponential_coordinates(Stheta2)) for _ in range(10): Stheta = random_state.randn(6) # ensure that theta is not within [-pi, pi] Stheta[random_state.randint(0, 3)] += np.pi + random_state.rand() Stheta_norm = norm_exponential_coordinates(Stheta) assert_false(np.all(Stheta == Stheta_norm)) A2B = transform_from_exponential_coordinates(Stheta) Stheta2 = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta2, Stheta_norm)
def test_transforms_from_exponential_coordinates(): A2B = np.eye(4) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) A2B2 = transforms_from_exponential_coordinates([Stheta])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0] assert_array_almost_equal(A2B, A2B2) A2B = translate_transform(np.eye(4), [1.0, 5.0, 0.0]) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0]) A2B2 = transforms_from_exponential_coordinates([Stheta])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0] assert_array_almost_equal(A2B, A2B2) A2B = rotate_transform(np.eye(4), active_matrix_from_angle(2, 0.5 * np.pi)) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0]) A2B2 = transforms_from_exponential_coordinates([Stheta])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0] assert_array_almost_equal(A2B, A2B2) random_state = np.random.RandomState(53) for _ in range(5): A2B = random_transform(random_state) Stheta = exponential_coordinates_from_transform(A2B) A2B2 = transforms_from_exponential_coordinates([Stheta])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0] assert_array_almost_equal(A2B, A2B2)
def test_dual_quaternion_from_screw_parameters(): q = np.zeros(3) s_axis = np.array([1, 0, 0]) h = np.inf theta = 0 dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta) assert_array_almost_equal(dq, np.array([1, 0, 0, 0, 0, 0, 0, 0])) q = np.zeros(3) s_axis = norm_vector(np.array([2.3, 2.4, 2.5])) h = np.inf theta = 3.6 dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta) pq = pq_from_dual_quaternion(dq) assert_array_almost_equal(pq, np.r_[s_axis * theta, 1, 0, 0, 0]) q = np.zeros(3) s_axis = norm_vector(np.array([2.4, 2.5, 2.6])) h = 0 theta = 4.1 dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta) pq = pq_from_dual_quaternion(dq) assert_array_almost_equal(pq[:3], [0, 0, 0]) assert_array_almost_equal(axis_angle_from_quaternion(pq[3:]), norm_axis_angle(np.r_[s_axis, theta])) random_state = np.random.RandomState(1001) for _ in range(5): A2B = random_transform(random_state) Stheta = exponential_coordinates_from_transform(A2B) S, theta = screw_axis_from_exponential_coordinates(Stheta) q, s_axis, h = screw_parameters_from_screw_axis(S) dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta) assert_unit_dual_quaternion(dq) dq_expected = dual_quaternion_from_transform(A2B) assert_unit_dual_quaternion_equal(dq, dq_expected)
and the green line corresponds to an interpolation with dual quaternions. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt import pytransform3d.transformations as pt 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 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( [pt.transform_from_dual_quaternion(dq) for dq in interpolated_dqs]) interpolated_ecs = (np.linspace(1, 0, n_steps)[:, np.newaxis] * Stheta1 + np.linspace(0, 1, n_steps)[:, np.newaxis] * Stheta2) interpolates_poses_from_ecs = ptr.transforms_from_exponential_coordinates( interpolated_ecs)
s = time_scaling(np.linspace(0.0, 5.0, 50001), 5.0) start = transform_from( R=active_matrix_from_angle(1, -0.4 * np.pi), p=np.array([-1, -2.5, 0]) ) goal1 = transform_from( R=active_matrix_from_angle(1, -0.1 * np.pi), p=np.array([-1, 1, 0]) ) goal2 = transform_from( R=active_matrix_from_angle(2, -np.pi), p=np.array([-0.65, -0.75, 0]) ) path1 = straight_line_path( exponential_coordinates_from_transform(start), exponential_coordinates_from_transform(goal1), s ) path2 = straight_line_path( exponential_coordinates_from_transform(goal1), exponential_coordinates_from_transform(goal2), s ) H = transforms_from_exponential_coordinates(np.vstack((path1, path2))) ax = make_3d_axis(1.0) trajectory = Trajectory(H, n_frames=1000, show_direction=False, s=0.3) trajectory.add_trajectory(ax) ax.view_init(azim=-95, elev=70) ax.set_xlim((-2.2, 1.3)) ax.set_ylim((-2.5, 1))
def straight_line_path(start, goal, s): """Compute straight line path in exponential coordinates.""" start = np.asarray(start) goal = np.asarray(goal) return (start[np.newaxis] * (1.0 - s)[:, np.newaxis] + goal[np.newaxis] * s[:, np.newaxis]) s = time_scaling(np.linspace(0.0, 5.0, 50001), 5.0) start = transform_from(R=active_matrix_from_angle(1, -0.4 * np.pi), p=np.array([-1, -2.5, 0])) goal1 = transform_from(R=active_matrix_from_angle(1, -0.1 * np.pi), p=np.array([-1, 1, 0])) goal2 = transform_from(R=active_matrix_from_angle(2, -np.pi), p=np.array([-0.65, -0.75, 0])) path1 = straight_line_path(exponential_coordinates_from_transform(start), exponential_coordinates_from_transform(goal1), s) path2 = straight_line_path(exponential_coordinates_from_transform(goal1), exponential_coordinates_from_transform(goal2), s) H = transforms_from_exponential_coordinates(np.vstack((path1, path2))) ax = make_3d_axis(1.0) trajectory = Trajectory(H, n_frames=1000, show_direction=False, s=0.3) trajectory.add_trajectory(ax) ax.view_init(azim=-95, elev=70) ax.set_xlim((-2.2, 1.3)) ax.set_ylim((-2.5, 1)) remove_frame(ax) plt.show()