def test_rotate_vector(self):
q = Quaternion(0.5, 0.5, 0.5, 0.5)
rotated_vector = q.rotate_vector(np.array([1., 2., 3.]))
expected_rotated_vector = np.array([3., 1., 2.])
npt.assert_allclose(rotated_vector, expected_rotated_vector)
def plot_poses(poses_list, plot_arrows=True, title="", blocking=True):
title_position = 1.05
fig = plt.figure()
plt.clf()
ax = Axes3D(fig)
if title:
fig.suptitle(title, fontsize='24')
colors = ['r', 'g', 'b', 'c', 'm', 'k']
num_colors = len(colors)
assert len(poses_list) < num_colors, (
"Need to define more colors to plot more trajectories!")
(bbox_max, bbox_min) = compute_bbox_3D(poses_list)
arrow_size = np.linalg.norm(bbox_max - bbox_min) * 0.05
arrow_width = 2
axis_min = np.amin(bbox_min)
axis_max = np.amax(bbox_max)
ax.set_xlim3d(axis_min, axis_max)
ax.set_ylim3d(axis_min, axis_max)
ax.set_zlim3d(axis_min, axis_max)
for i in range(0, len(poses_list)):
poses = poses_list[i].copy()
# Plot line.
positions = ax.plot(xs=poses[:, 0],
ys=poses[:, 1],
zs=poses[:, 2],
color=colors[i])
for pose in poses:
# Position point
ax.plot([pose[0]], [pose[1]], [pose[2]],
'o',
markersize=5,
color=colors[i],
alpha=0.5)
if not plot_arrows:
continue
rotation_quaternion = Quaternion(q=pose[3:7])
x_rotated = rotation_quaternion.rotate_vector([1, 0, 0, 0])
x_rotated *= arrow_size
a = Arrow3D([pose[0], pose[0] + x_rotated[0]],
[pose[1], pose[1] + x_rotated[1]],
[pose[2], pose[2] + x_rotated[2]],
mutation_scale=20,
lw=arrow_width,
arrowstyle="-|>",
color="r")
ax.add_artist(a)
y_rotated = rotation_quaternion.rotate_vector([0, 1, 0, 0])
y_rotated *= arrow_size
a = Arrow3D([pose[0], pose[0] + y_rotated[0]],
[pose[1], pose[1] + y_rotated[1]],
[pose[2], pose[2] + y_rotated[2]],
mutation_scale=20,
lw=arrow_width,
arrowstyle="-|>",
color="g")
ax.add_artist(a)
z_rotated = rotation_quaternion.rotate_vector([0, 0, 1, 0])
z_rotated *= arrow_size
a = Arrow3D([pose[0], pose[0] + z_rotated[0]],
[pose[1], pose[1] + z_rotated[1]],
[pose[2], pose[2] + z_rotated[2]],
mutation_scale=20,
lw=arrow_width,
arrowstyle="-|>",
color="b")
ax.add_artist(a)
ax.auto_scale_xyz([axis_min, axis_max], [axis_min, axis_max],
[axis_min, axis_max])
plt.show(block=blocking)