def test_quaternion_slerp_batch():
random_state = np.random.RandomState(229)
q_start = pr.random_quaternion(random_state)
q_end = pr.random_quaternion(random_state)
t = np.linspace(0, 1, 101)
Q = pbr.quaternion_slerp_batch(q_start, q_end, t)
for i in range(len(t)):
qi = pr.quaternion_slerp(q_start, q_end, t[i])
pr.assert_quaternion_equal(Q[i], qi)
def update_lines(step, start, end, n_frames, rot, profile):
global velocity
global last_R
if step == 0:
velocity = []
last_R = pr.matrix_from_quaternion(start)
if step <= n_frames / 2:
t = step / float(n_frames / 2 - 1)
q = pr.quaternion_slerp(start, end, t)
else:
t = (step - n_frames / 2) / float(n_frames / 2 - 1)
q = interpolate_linear(end, start, t)
R = pr.matrix_from_quaternion(q)
# Draw new frame
rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]])
rot[0].set_3d_properties([0, R[2, 0]])
rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]])
rot[1].set_3d_properties([0, R[2, 1]])
rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]])
rot[2].set_3d_properties([0, R[2, 2]])
# Update vector in frame
test = R.dot(np.ones(3) / np.sqrt(3.0))
rot[3].set_data(np.array([test[0] / 2.0, test[0]]),
[test[1] / 2.0, test[1]])
rot[3].set_3d_properties([test[2] / 2.0, test[2]])
velocity.append(np.linalg.norm(R - last_R))
last_R = R
profile.set_data(np.linspace(0, 1, n_frames)[:len(velocity)], velocity)
return rot
"""
===============
Pose Trajectory
===============
Plotting pose trajectories with pytransform3d is easy.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import q_id, quaternion_slerp
from pytransform3d.trajectories import plot_trajectory
n_steps = 100
P = np.empty((n_steps, 7))
P[:, 0] = np.cos(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))
P[:, 1] = np.sin(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))
P[:, 2] = np.linspace(-1, 1, n_steps)
q_end = np.array([0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)])
P[:, 3:] = np.vstack(
[quaternion_slerp(q_id, q_end, t) for t in np.linspace(0, 1, n_steps)])
ax = plot_trajectory(P=P, s=0.3, lw=2, c="k")
plt.show()
R2_end = matrix_from_axis_angle([1, 0, 0, end_angle])
q_start = quaternion_from_matrix(R1.dot(R2_start))
q_end = quaternion_from_matrix(R1.dot(R2_end))
Y = np.zeros((len(T), 7))
Y[:, 0] = radius * np.cos(np.deg2rad(90) - end_angle * sigmoid)
Y[:, 2] = radius * np.sin(np.deg2rad(90) - end_angle * sigmoid)
if end_angle > np.pi:
q_end *= -1.0
Y[:, 3:] = (1.0 - T)[:, np.newaxis] * q_start + T[:, np.newaxis] * q_end
Y_slerp = np.zeros((len(T), 7))
Y_slerp[:, 0] = 0.7 * radius * np.cos(np.deg2rad(90) - end_angle * sigmoid)
Y_slerp[:, 2] = 0.7 * radius * np.sin(np.deg2rad(90) - end_angle * sigmoid)
for i, t in enumerate(T):
Y_slerp[i, 3:] = quaternion_slerp(q_start, q_end, t)
ax = plot_trajectory(P=Y[:, :7],
show_direction=False,
n_frames=40,
s=0.05,
ax_s=0.7)
ax = plot_trajectory(P=Y_slerp[:, :7],
show_direction=False,
n_frames=40,
s=0.05,
ax=ax)
ax.text(0.1, 0, 0, "SLERP")
ax.text(0.4, 0, 0.6, "Naive linear interpolation")
ax.view_init(elev=10, azim=90)
plt.show()
