From ba64fa9e690dfcd059707d57eb0f1d959474d444 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Thu, 28 Sep 2023 10:36:39 +0200 Subject: [PATCH] Extend example --- .../plot_dual_quaternion_interpolation.py | 32 ++++++++++++++----- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/examples/plots/plot_dual_quaternion_interpolation.py b/examples/plots/plot_dual_quaternion_interpolation.py index 32c70dce2..bc8f498f9 100644 --- a/examples/plots/plot_dual_quaternion_interpolation.py +++ b/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,8 +53,8 @@ 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 + @@ -60,20 +62,34 @@ 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()