""" ==================================================== 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()
R=active_matrix_from_angle(1, -0.4 * np.pi),
p=np.array([-1, -2.5, 0])
)
goal1 = transform_from(
R=active_matrix_from_angle(1, -0.1 * np.pi),
p=np.array([-1, 1, 0])
)
goal2 = transform_from(
R=active_matrix_from_angle(2, -np.pi),
p=np.array([-0.65, -0.75, 0])
)
path1 = straight_line_path(
exponential_coordinates_from_transform(start),
exponential_coordinates_from_transform(goal1),
s
)
path2 = straight_line_path(
exponential_coordinates_from_transform(goal1),
exponential_coordinates_from_transform(goal2),
s
)
H = transforms_from_exponential_coordinates(np.vstack((path1, path2)))
ax = make_3d_axis(1.0)
trajectory = Trajectory(H, n_frames=1000, show_direction=False, s=0.3)
trajectory.add_trajectory(ax)
ax.view_init(azim=-95, elev=70)
ax.set_xlim((-2.2, 1.3))
ax.set_ylim((-2.5, 1))
remove_frame(ax)
plt.show()
""" =================== Plot Transformation =================== We can display transformations by plotting the basis vectors of the corresponding coordinate frame. """ print(__doc__) import matplotlib.pyplot as plt from pytransform3d.transformations import plot_transform from pytransform3d.plot_utils import make_3d_axis ax = make_3d_axis(ax_s=1, unit="m", n_ticks=6) plot_transform(ax=ax) plt.tight_layout() plt.show()
sensor_size = (0.00367, 0.00274)
image_size = (640, 480)
intrinsic_camera_matrix = np.array([[focal_length, 0, sensor_size[0] / 2],
[0, focal_length, sensor_size[1] / 2],
[0, 0, 1]])
world_grid = make_world_grid(n_points_per_line=101)
image_grid = world2image(world_grid,
cam2world,
sensor_size,
image_size,
focal_length,
kappa=0.4)
plt.figure(figsize=(12, 5))
ax = make_3d_axis(1, 121, unit="m")
ax.view_init(elev=30, azim=-70)
plot_transform(ax)
plot_transform(ax, A2B=cam2world, s=0.3, name="Camera")
plot_camera(ax,
intrinsic_camera_matrix,
cam2world,
sensor_size=sensor_size,
virtual_image_distance=0.5)
ax.set_title("Camera and world frames")
ax.scatter(world_grid[:, 0],
world_grid[:, 1],
world_grid[:, 2],
s=1,
alpha=0.2)
ax.scatter(world_grid[-1, 0], world_grid[-1, 1], world_grid[-1, 2], color="r")
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.plot_utils import make_3d_axis
from pytransform3d.transformations import random_transform
from pytransform3d.transform_manager import TransformManager
random_state = np.random.RandomState(0)
A2world = random_transform(random_state)
B2world = random_transform(random_state)
A2C = random_transform(random_state)
D2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "world", A2world)
tm.add_transform("B", "world", B2world)
tm.add_transform("A", "C", A2C)
tm.add_transform("D", "B", D2B)
plt.figure(figsize=(10, 5))
ax = make_3d_axis(3, 121)
ax = tm.plot_frames_in("world", ax=ax, alpha=0.6)
ax.view_init(30, 20)
ax = make_3d_axis(3, 122)
ax = tm.plot_frames_in("A", ax=ax, alpha=0.6)
ax.view_init(30, 20)
plt.show()