def test_quaternions_from_matrices_no_batch():
random_state = np.random.RandomState(85)
for _ in range(5):
q = pr.random_quaternion(random_state)
R = pr.matrix_from_quaternion(q)
q2 = pbr.quaternions_from_matrices(R)
pr.assert_quaternion_equal(q, q2)
a = np.array([1.0, 0.0, 0.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
a = np.array([0.0, 1.0, 0.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
a = np.array([0.0, 0.0, 1.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
def test_quaternions_from_matrices_4d():
random_state = np.random.RandomState(84)
for _ in range(5):
q = pr.random_quaternion(random_state)
R = pr.matrix_from_quaternion(q)
q2 = pbr.quaternions_from_matrices([[R, R], [R, R]])
pr.assert_quaternion_equal(q, q2[0, 0])
pr.assert_quaternion_equal(q, q2[0, 1])
pr.assert_quaternion_equal(q, q2[1, 0])
pr.assert_quaternion_equal(q, q2[1, 1])
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
def update_lines(step, Q, rot):
R = pr.matrix_from_quaternion(Q[step])
# 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]])
return rot
p = np.array([1.0, 1.0, 1.0]) euler = [0, 0, 0] euler[axis] = angle R = pr.active_matrix_from_intrinsic_euler_xyz(euler) pr.plot_basis(ax, R, p) p = np.array([-1.0, 1.0, 1.0]) euler = [0, 0, 0] euler[2 - axis] = angle R = pr.active_matrix_from_intrinsic_euler_zyx(euler) pr.plot_basis(ax, R, p) p = np.array([1.0, 1.0, -1.0]) R = pr.active_matrix_from_angle(axis, angle) pr.plot_basis(ax, R, p) p = np.array([1.0, -1.0, -1.0]) e = [pr.unitx, pr.unity, pr.unitz][axis] a = np.hstack((e, (angle, ))) R = pr.matrix_from_axis_angle(a) pr.plot_basis(ax, R, p) pr.plot_axis_angle(ax, a, p) p = np.array([-1.0, -1.0, -1.0]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_quaternion(q) pr.plot_basis(ax, R, p) plt.show()
end = np.array([0, 0, 0, np.pi])
end[:3] = np.random.randn(3)
end = pr.quaternion_from_axis_angle(end)
n_frames = 200
fig = plt.figure(figsize=(12, 5))
ax = fig.add_subplot(121, projection="3d")
ax.set_xlim((-1, 1))
ax.set_ylim((-1, 1))
ax.set_zlim((-1, 1))
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
Rs = pr.matrix_from_quaternion(start)
Re = pr.matrix_from_quaternion(end)
rot = [
ax.plot([0, 1], [0, 0], [0, 0], c="r", lw=3)[0],
ax.plot([0, 0], [0, 1], [0, 0], c="g", lw=3)[0],
ax.plot([0, 0], [0, 0], [0, 1], c="b", lw=3)[0],
ax.plot([0, 1], [0, 1], [0, 1], c="gray", lw=3)[0],
ax.plot([0, Rs[0, 0]], [0, Rs[1, 0]], [0, Rs[2, 0]],
c="r",
lw=3,
alpha=0.5)[0],
ax.plot([0, Rs[0, 1]], [0, Rs[1, 1]], [0, Rs[2, 1]],
c="g",
lw=3,
alpha=0.5)[0],
angular_velocities = np.vstack((np.zeros(
(1, 3)), np.cumsum(angular_accelerations * dt, axis=0)))
# Integrate angular velocities to quaternions
Q = pr.quaternion_integrate(angular_velocities, q0=start, dt=dt)
fig = plt.figure(figsize=(4, 3))
ax = fig.add_subplot(111, projection="3d")
ax.set_xlim((-1, 1))
ax.set_ylim((-1, 1))
ax.set_zlim((-1, 1))
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
R = pr.matrix_from_quaternion(start)
rot = [
ax.plot([0, 1], [0, 0], [0, 0], c="r", lw=3)[0],
ax.plot([0, 0], [0, 1], [0, 0], c="g", lw=3)[0],
ax.plot([0, 0], [0, 0], [0, 1], c="b", lw=3)[0],
ax.plot([0, R[0, 0]], [0, R[1, 0]], [0, R[2, 0]],
c="r",
lw=3,
alpha=0.3)[0],
ax.plot([0, R[0, 1]], [0, R[1, 1]], [0, R[2, 1]],
c="g",
lw=3,
alpha=0.3)[0],
ax.plot([0, R[0, 2]], [0, R[1, 2]], [0, R[2, 2]],
c="b",
"""
======================
Quaternion Integration
======================
Integrate angular velocities to a sequence of quaternions.
"""
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import (quaternion_integrate,
matrix_from_quaternion, plot_basis)
angular_velocities = np.empty((21, 3))
angular_velocities[:, :] = np.array([np.sqrt(0.5), np.sqrt(0.5), 0.0])
angular_velocities *= np.pi
Q = quaternion_integrate(angular_velocities, dt=0.1)
ax = None
for t in range(len(Q)):
R = matrix_from_quaternion(Q[t])
p = 2 * (t / (len(Q) - 1) - 0.5) * np.ones(3)
ax = plot_basis(ax=ax, s=0.15, R=R, p=p)
plt.show()
