def test_conversions_between_exponential_coordinates_and_transform():
A2B = np.eye(4)
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
A2B2 = transform_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B = translate_transform(np.eye(4), [1.0, 5.0, 0.0])
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0])
A2B2 = transform_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B = rotate_transform(np.eye(4), active_matrix_from_angle(2, 0.5 * np.pi))
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0])
A2B2 = transform_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
random_state = np.random.RandomState(52)
for _ in range(5):
A2B = random_transform(random_state)
Stheta = exponential_coordinates_from_transform(A2B)
A2B2 = transform_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
def test_transforms_from_exponential_coordinates():
A2B = np.eye(4)
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
A2B2 = transforms_from_exponential_coordinates([Stheta])[0]
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0]
assert_array_almost_equal(A2B, A2B2)
A2B = translate_transform(np.eye(4), [1.0, 5.0, 0.0])
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0])
A2B2 = transforms_from_exponential_coordinates([Stheta])[0]
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0]
assert_array_almost_equal(A2B, A2B2)
A2B = rotate_transform(np.eye(4), active_matrix_from_angle(2, 0.5 * np.pi))
Stheta = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0])
A2B2 = transforms_from_exponential_coordinates([Stheta])[0]
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0]
assert_array_almost_equal(A2B, A2B2)
random_state = np.random.RandomState(53)
for _ in range(5):
A2B = random_transform(random_state)
Stheta = exponential_coordinates_from_transform(A2B)
A2B2 = transforms_from_exponential_coordinates([Stheta])[0]
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates(Stheta)
assert_array_almost_equal(A2B, A2B2)
A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0,
0]
assert_array_almost_equal(A2B, A2B2)
def plot_screw(figure,
q=np.zeros(3),
s_axis=np.array([1.0, 0.0, 0.0]),
h=1.0,
theta=1.0,
A2B=None,
s=1.0):
"""Plot transformation about and along screw axis.
Parameters
----------
figure : Figure
Interface to Open3D's visualizer
q : array-like, shape (3,), optional (default: [0, 0, 0])
Vector to a point on the screw axis
s_axis : array-like, shape (3,), optional (default: [1, 0, 0])
Direction vector of the screw axis
h : float, optional (default: 1)
Pitch of the screw. The pitch is the ratio of translation and rotation
of the screw axis. Infinite pitch indicates pure translation.
theta : float, optional (default: 1)
Rotation angle. h * theta is the translation.
A2B : array-like, shape (4, 4), optional (default: I)
Origin of the screw
s : float, optional (default: 1)
Scaling of the axis and angle that will be drawn
"""
from pytransform3d.rotations import (vector_projection,
angle_between_vectors,
perpendicular_to_vectors,
slerp_weights)
from pytransform3d.transformations import (check_screw_parameters,
transform, translate_transform,
vector_to_point,
vector_to_direction,
vectors_to_points)
if A2B is None:
A2B = np.eye(4)
q, s_axis, h = check_screw_parameters(q, s_axis, h)
origin_projected_on_screw_axis = q + vector_projection(-q, s_axis)
pure_translation = np.isinf(h)
if not pure_translation:
screw_axis_to_old_frame = -origin_projected_on_screw_axis
screw_axis_to_rotated_frame = perpendicular_to_vectors(
s_axis, screw_axis_to_old_frame)
screw_axis_to_translated_frame = h * s_axis
arc = np.empty((100, 3))
angle = angle_between_vectors(screw_axis_to_old_frame,
screw_axis_to_rotated_frame)
for i, t in enumerate(
zip(np.linspace(0, 2 * theta / np.pi, len(arc)),
np.linspace(0.0, 1.0, len(arc)))):
t1, t2 = t
w1, w2 = slerp_weights(angle, t1)
arc[i] = (origin_projected_on_screw_axis +
w1 * screw_axis_to_old_frame +
w2 * screw_axis_to_rotated_frame +
screw_axis_to_translated_frame * t2 * theta)
q = transform(A2B, vector_to_point(q))[:3]
s_axis = transform(A2B, vector_to_direction(s_axis))[:3]
if not pure_translation:
arc = transform(A2B, vectors_to_points(arc))[:, :3]
origin_projected_on_screw_axis = transform(
A2B, vector_to_point(origin_projected_on_screw_axis))[:3]
# Screw axis
Q = translate_transform(np.eye(4), q)
fig.plot_sphere(radius=s * 0.02, A2B=Q, c=[1, 0, 0])
if pure_translation:
s_axis *= theta
Q_plus_S_axis = translate_transform(np.eye(4), q + s_axis)
fig.plot_sphere(radius=s * 0.02, A2B=Q_plus_S_axis, c=[0, 1, 0])
P = np.array(
[[q[0] - s * s_axis[0], q[1] - s * s_axis[1], q[2] - s * s_axis[2]],
[
q[0] + (1 + s) * s_axis[0], q[1] + (1 + s) * s_axis[1],
q[2] + (1 + s) * s_axis[2]
]])
figure.plot(P=P, c=[0, 0, 0])
if not pure_translation:
# Transformation
figure.plot(arc, c=[0, 0, 0])
for i, c in zip([0, -1], [[1, 0, 0], [0, 1, 0]]):
arc_bound = np.vstack((origin_projected_on_screw_axis, arc[i]))
figure.plot(arc_bound, c=c)