""" ==================================================== Axis-Angle Representation from Two Direction Vectors ==================================================== This example shows how we can compute the axis-angle representation of a rotation that transforms a direction given by a vector 'a' to a direction given by a vector 'b'. We show both vectors, the rotation about the rotation axis and the initial and resulting coordinate frame, where the vector 'b' and its corresponding frame after the rotation are represented by shorter lines. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt from pytransform3d.rotations import axis_angle_from_two_directions, matrix_from_axis_angle, plot_axis_angle, plot_basis from pytransform3d.plot_utils import make_3d_axis, plot_vector a = np.array([1.0, 0.0, 0.0]) b = np.array([0.76958075, -0.49039301, -0.40897453]) aa = axis_angle_from_two_directions(a, b) ax = make_3d_axis(ax_s=1) plot_vector(ax, start=np.zeros(3), direction=a, s=1.0) plot_vector(ax, start=np.zeros(3), direction=b, s=0.5) plot_axis_angle(ax, aa) plot_basis(ax) plot_basis(ax, R=matrix_from_axis_angle(aa), s=0.5) plt.show()
""" ======================================== Compare Various Definitions of Rotations ======================================== """ print(__doc__) import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from pytransform3d import rotations as pr ax = pr.plot_basis(R=np.eye(3), ax_s=2, lw=3) axis = 2 angle = np.pi 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)
import numpy as np import matplotlib.pyplot as plt from pytransform3d.rotations import matrix_from_two_vectors, plot_basis, random_vector from pytransform3d.plot_utils import plot_vector random_state = np.random.RandomState(1) a = random_vector(random_state, 3) * 0.3 b = random_vector(random_state, 3) * 0.3 R = matrix_from_two_vectors(a, b) ax = plot_vector(direction=a, color="r") plot_vector(ax=ax, direction=b, color="g") plot_basis(ax=ax, R=R) plt.show()
def visualize_results():
plt.figure(200)
# plot time
plt.subplot(1, 2, 1)
plt.plot(time_ref[:])
plt.xlabel('Data points')
plt.ylabel('Time (s)')
plt.title("Time series")
# plt.legend()
plt.grid(True)
# plot motor rates
plt.subplot(1, 2, 2)
plt.plot(time_ref[:], desired_rotor_rates[:, 0], label='Motor 1')
# plt.plot(time_ref[:], desired_rotor_rates[:,1], label='Motor 2')
# plt.plot(time_ref[:], desired_rotor_rates[:,2], label='Motor 3')
# plt.plot(time_ref[:], desired_rotor_rates[:,3], label='Motor 4')
plt.xlabel('Sim time (s)')
plt.ylabel('Motor rate (rad/s)')
plt.title("Motor rates over time")
plt.legend()
plt.grid(True)
plt.figure(300)
# plot desired moments
plt.subplot(1, 2, 1)
plt.plot(time_ref[:], desired_moments[:, 0], label="roll")
# plt.plot(time_ref[:], desired_moments[:,1], label="pitch")
# plt.plot(time_ref[:], desired_moments[:,2], label="yaw")
plt.xlabel('Sim time (s)')
plt.ylabel('Moments (N)')
plt.title("Desired moments over time")
plt.legend()
plt.grid(True)
###
#
# PLOT THE COORDINATE AXIS ROTATING OVER TIME --- continue looking for spikes in data / numerical issues
#
###
# https://rock-learning.github.io/pytransform3d/rotations.html
#
# TODO: can also plot the error in rotation matrix properties, eg: R orthonormal, R^T = R^-1, det(R) = 1
#
# deriving euler angles from rotation matrix
# https://www.gregslabaugh.net/publications/euler.pdf
#
# TODO: my time derivatives are probably wrong, this is a fundamental math issue. I need to work these derivatives on paper
#
# TODO: the Rc rotations are plotted entirely wrong, when the quad rolls right and falls this appears as roll left and rise
# I'm not sure if this is just desired behavior though?
plt.figure(400)
ax = plot_basis(R=np.eye(3), ax_s=2)
scaling = 75
for i in range(math.floor(len(test_derived_rot_resize) / scaling)):
plot_basis(ax=ax,
R=test_derived_rot_resize[:, :][i * scaling],
p=actual_pos[i * scaling])
ax.set_xlabel("x-axis (m)")
ax.set_ylabel("y-axis (m)")
ax.set_zlabel("z-axis (m)")
plt.title("Body basis vectors over time")
plt.grid(True)
plt.figure(500)
ax = plot_basis(R=np.eye(3), ax_s=2)
scaling = 75
for i in range(math.floor(len(test_derived_rot_resize) / scaling)):
plot_basis(ax=ax,
R=test_Rc_resize[:, :][i * scaling],
p=actual_pos[i * scaling]) # FAILS INVERSION TEST
#
# Plot "trajectory", doesn't look great
# combined_traj = np.concatenate((actual_pos, actual_att), axis=1)
# plot_trajectory(ax=ax, P=combined_traj)
ax.set_xlabel("x-axis (m)")
ax.set_ylabel("y-axis (m)")
ax.set_zlabel("z-axis (m)")
plt.title("Rc over time")
plt.grid(True)
plt.tight_layout()
plt.show()
"""
=====================================
Axis-Angle Representation of Rotation
=====================================
Any rotation can be represented with a single rotation about some axis.
Here we see a frame that is rotated in multiple steps around a rotation
axis.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import (random_axis_angle, matrix_from_axis_angle,
plot_basis, plot_axis_angle)
original = random_axis_angle(np.random.RandomState(5))
ax = plot_axis_angle(a=original)
for fraction in np.linspace(0, 1, 50):
a = original.copy()
a[-1] = fraction * original[-1]
R = matrix_from_axis_angle(a)
plot_basis(ax, R, alpha=0.2)
plt.subplots_adjust(left=0, right=1, bottom=0, top=1.1)
ax.view_init(azim=105, elev=12)
plt.show()
Any rotation can be represented by three consecutive rotations about three
basis vectors. Here we use the extrinsic xyz convention.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d import rotations as pr
from pytransform3d.plot_utils import remove_frame
alpha, beta, gamma = 0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi
p = np.array([1, 1, 1])
plt.figure(figsize=(5, 5))
ax = pr.plot_basis(R=np.eye(3), p=-1.5 * p, ax_s=2)
pr.plot_axis_angle(ax, [1, 0, 0, alpha], -1.5 * p)
pr.plot_basis(ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, 0, 0]),
-0.5 * p)
pr.plot_axis_angle(ax, [0, 1, 0, beta], p=-0.5 * p)
pr.plot_basis(ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, 0]),
0.5 * p)
pr.plot_axis_angle(ax, [0, 0, 1, gamma], 0.5 * p)
pr.plot_basis(ax,
pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, gamma]),
1.5 * p,
lw=5)
input0 = (0, 0, np.deg2rad(-90))
output0 = (0, np.deg2rad(-90), 0)
input1 = (np.deg2rad(-90), 0, np.deg2rad(90))
output1 = (np.deg2rad(-90), np.deg2rad(90), 0)
input2 = (0, np.deg2rad(-90), 0)
output2 = (0, 0, np.deg2rad(90))
Rx90 = passive_matrix_from_angle(0, np.deg2rad(90))
Ry90 = passive_matrix_from_angle(1, np.deg2rad(90))
Rz90 = passive_matrix_from_angle(2, np.deg2rad(90))
_Rx90 = passive_matrix_from_angle(0, np.deg2rad(-90))
_Ry90 = passive_matrix_from_angle(1, np.deg2rad(-90))
_Rz90 = passive_matrix_from_angle(2, np.deg2rad(-90))
plot_basis(ax=axes[0], R=np.eye(3))
plot_basis(ax=axes[1], R=Rx90)
plt.show()
Rx45 = passive_matrix_from_angle(0, np.deg2rad(45))
Rz45 = passive_matrix_from_angle(2, np.deg2rad(45))
axes[0].set_title("Passive Extrinsic Rotations", y=0.95)
plot_basis(ax=axes[0], R=np.eye(3))
axes[1].set_title("$R_x(45^{\circ})$", y=0.95)
plot_basis(ax=axes[1], R=Rx45)
axes[2].set_title("$R_z(45^{\circ}) R_x(45^{\circ})$", y=0.95)
plot_basis(ax=axes[2], R=Rz45.dot(Rx45))
axes[3].set_title("Passive Intrinsic Rotations", y=0.95)
plot_basis(ax=axes[3], R=np.eye(3))
"""
======================
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()
ax.add_artist(a)
a = Arrow3D([0, 0], [0, 0], [0, 1], **arrow_prop_dict, color='r')
ax.add_artist(a)
# Give them a name:
# ax.text(0.0, 0.0, -0.1, r'$0$')
# ax.text(1.1, 0, 0, r'$x$')
# ax.text(0, 1.1, 0, r'$z$')
# ax.text(0, 0, 1.1, r'$y$')
angle = np.pi / 2
p = np.array([0.09870368, 0.42568716, -0.23324492])
R = np.matrix([[-0.06576756, -0.05730691, -0.99618801],
[0.99761943, -0.02452605, -0.06445117],
[-0.02073906, -0.9980553, 0.05878351]])
plot_basis(ax, R, p)
ax.invert_zaxis()
plt.show()
elif plot_camera_2d:
# reference: https://github.com/rlabbe/filterpy/blob/master/filterpy/stats/tests/test_stats.py
input_dir = 'cov_results_camera/'
cov_files = []
for (dirpath, dirnames, filenames) in walk(input_dir):
for filename in filenames:
if filename.endswith('cov.npy'):
cov_files.append(os.path.join(dirpath, filename))
