Example #1
0
"""
====================================================
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()
Example #2
0
    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()
Example #3
0
"""
===================
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")
Example #5
0
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()