def test_dual_quaternion_applied_to_point():
random_state = np.random.RandomState(1000)
for _ in range(5):
p_A = random_vector(random_state, 3)
A2B = random_transform(random_state)
dq = dual_quaternion_from_transform(A2B)
p_B = dq_prod_vector(dq, p_A)
assert_array_almost_equal(p_B,
transform(A2B, vector_to_point(p_A))[:3])
def test_vectors_to_points():
"""Test conversion from vectors to homogenous coordinates."""
V = np.array([[1, 2, 3], [2, 3, 4]])
PA = vectors_to_points(V)
assert_array_almost_equal(PA, [[1, 2, 3, 1], [2, 3, 4, 1]])
random_state = np.random.RandomState(0)
V = random_state.randn(10, 3)
for i, p in enumerate(vectors_to_points(V)):
assert_array_almost_equal(p, vector_to_point(V[i]))
def test_vector_to_point():
"""Test conversion from vector to homogenous coordinates."""
v = np.array([1, 2, 3])
pA = vector_to_point(v)
assert_array_almost_equal(pA, [1, 2, 3, 1])
random_state = np.random.RandomState(0)
R = matrix_from(a=random_axis_angle(random_state))
p = random_vector(random_state)
A2B = transform_from(R, p)
assert_transform(A2B)
_ = transform(A2B, pA)
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)