Extend example

examples/plots/plot_dual_quaternion_interpolation.py

@@ -24,6 +24,8 @@
 dq2 = -pt.dual_quaternion_from_transform(pose2)
 Stheta1 = pt.exponential_coordinates_from_transform(pose1)
 Stheta2 = pt.exponential_coordinates_from_transform(pose2)
+pq1 = pt.pq_from_transform(pose1)
+pq2 = pt.pq_from_transform(pose2)
 
 n_steps = 100
 
@@ -51,29 +53,43 @@
 sclerp_interpolated_dqs = np.vstack([
     pt.dual_quaternion_sclerp(dq1, dq2, t)
     for t in np.linspace(0, 1, n_steps)])
-sclerp_interpolated_poses_from_dqs = np.array([
-    pt.transform_from_dual_quaternion(dq) for dq in sclerp_interpolated_dqs])
+sclerp_interpolated_poses_from_dqs = ptr.transforms_from_dual_quaternions(
+    sclerp_interpolated_dqs)
 
 # Linear interpolation of exponential coordinates
 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)
 
+# Linear interpolation of position + spherical linear interpolation (SLERP) of
+# quaternion
+interpolated_pqs = np.vstack([
+    pt.pq_slerp(pq1, pq2, t) for t in np.linspace(0, 1, n_steps)])
+interpolated_poses_from_pqs = ptr.transforms_from_pqs(interpolated_pqs)
+
 ax = pt.plot_transform(A2B=pose1, s=0.3, ax_s=2)
 pt.plot_transform(A2B=pose2, s=0.3, ax=ax)
-traj = ppu.Trajectory(interpolated_poses, s=0.1, c="k", lw=5, alpha=0.5)
+traj = ppu.Trajectory(
+    interpolated_poses, s=0.1, c="k", lw=5, alpha=0.5, show_direction=True)
 traj.add_trajectory(ax)
-traj_from_dqs = ppu.Trajectory(interpolated_poses_from_dqs, s=0.1, c="g")
+traj_from_dqs = ppu.Trajectory(
+    interpolated_poses_from_dqs, s=0.1, c="g", show_direction=False)
 traj_from_dqs.add_trajectory(ax)
-traj_from_ecs = ppu.Trajectory(interpolates_poses_from_ecs, s=0.1, c="r")
+traj_from_ecs = ppu.Trajectory(
+    interpolates_poses_from_ecs, s=0.1, c="r", show_direction=False)
 traj_from_ecs.add_trajectory(ax)
 traj_from_dqs_sclerp = ppu.Trajectory(
-    sclerp_interpolated_poses_from_dqs, s=0.1, c="b")
+    sclerp_interpolated_poses_from_dqs, s=0.1, c="b", show_direction=False)
 traj_from_dqs_sclerp.add_trajectory(ax)
+traj_from_pq_slerp = ppu.Trajectory(
+    interpolated_poses_from_pqs, s=0.1, c="c", show_direction=False)
+traj_from_pq_slerp.add_trajectory(ax)
 plt.legend(
     [traj.trajectory, traj_from_dqs.trajectory, traj_from_ecs.trajectory,
-     traj_from_dqs_sclerp.trajectory],
+     traj_from_dqs_sclerp.trajectory, traj_from_pq_slerp.trajectory],
     ["Screw interpolation", "Linear dual quaternion interpolation",
-     "Linear interpolation of exp. coordinates", "Dual quaternion ScLERP"])
+     "Linear interpolation of exp. coordinates", "Dual quaternion ScLERP",
+     "Linear interpolation of position + SLERP of quaternions"],
+    loc="best")
 plt.show()