def test_conversions_between_screw_axis_and_parameters():
random_state = np.random.RandomState(98)
q = random_vector(random_state, 3)
s_axis = norm_vector(random_vector(random_state, 3))
h = np.inf
screw_axis = screw_axis_from_screw_parameters(q, s_axis, h)
assert_array_almost_equal(screw_axis, np.r_[0, 0, 0, s_axis])
q2, s_axis2, h2 = screw_parameters_from_screw_axis(screw_axis)
assert_array_almost_equal(np.zeros(3), q2)
assert_array_almost_equal(s_axis, s_axis2)
assert_array_almost_equal(h, h2)
for _ in range(10):
s_axis = norm_vector(random_vector(random_state, 3))
# q has to be orthogonal to s_axis so that we reconstruct it exactly
q = perpendicular_to_vector(s_axis)
h = random_state.rand() + 0.5
screw_axis = screw_axis_from_screw_parameters(q, s_axis, h)
q2, s_axis2, h2 = screw_parameters_from_screw_axis(screw_axis)
assert_array_almost_equal(q, q2)
assert_array_almost_equal(s_axis, s_axis2)
assert_array_almost_equal(h, h2)
def test_conversions_between_dual_quternion_and_pq():
random_state = np.random.RandomState(1000)
for _ in range(5):
pq = random_vector(random_state, 7)
pq[3:] /= np.linalg.norm(pq[3:])
dq = dual_quaternion_from_pq(pq)
pq2 = pq_from_dual_quaternion(dq)
assert_array_almost_equal(pq, pq2)
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_invert_transform():
"""Test inversion of transformations."""
random_state = np.random.RandomState(0)
for _ in range(5):
R = matrix_from(a=random_axis_angle(random_state))
p = random_vector(random_state)
A2B = transform_from(R, p)
B2A = invert_transform(A2B)
A2B2 = np.linalg.inv(B2A)
assert_array_almost_equal(A2B, A2B2)
def test_vector_to_direction():
"""Test conversion from vector to direction in homogenous coordinates."""
v = np.array([1, 2, 3])
dA = vector_to_direction(v)
assert_array_almost_equal(dA, [1, 2, 3, 0])
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, dA)
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)
pB = transform(A2B, pA)
def test_check_screw_axis():
random_state = np.random.RandomState(73)
omega = norm_vector(random_vector(random_state, 3))
v = random_vector(random_state, 3)
assert_raises_regexp(ValueError, "Expected 3D vector with shape",
check_screw_axis, np.r_[0, 1, v])
assert_raises_regexp(ValueError,
"Norm of rotation axis must either be 0 or 1",
check_screw_axis, np.r_[2 * omega, v])
assert_raises_regexp(ValueError, "If the norm of the rotation axis is 0",
check_screw_axis, np.r_[0, 0, 0, v])
S_pure_translation = np.r_[0, 0, 0, norm_vector(v)]
S = check_screw_axis(S_pure_translation)
assert_array_almost_equal(S, S_pure_translation)
S_both = np.r_[omega, v]
S = check_screw_axis(S_both)
assert_array_almost_equal(S, S_both)
def test_conversions_between_dual_quternion_and_transform():
random_state = np.random.RandomState(1000)
for _ in range(5):
A2B = random_transform(random_state)
dq = dual_quaternion_from_transform(A2B)
A2B2 = transform_from_dual_quaternion(dq)
assert_array_almost_equal(A2B, A2B2)
dq2 = dual_quaternion_from_transform(A2B2)
assert_unit_dual_quaternion_equal(dq, dq2)
for _ in range(5):
p = random_vector(random_state, 3)
q = random_quaternion(random_state)
dq = dual_quaternion_from_pq(np.hstack((p, q)))
A2B = transform_from_dual_quaternion(dq)
dq2 = dual_quaternion_from_transform(A2B)
assert_unit_dual_quaternion_equal(dq, dq2)
A2B2 = transform_from_dual_quaternion(dq2)
assert_array_almost_equal(A2B, A2B2)
def test_conversions_between_transform_and_transform_log():
A2B = np.eye(4)
transform_log = transform_log_from_transform(A2B)
assert_array_almost_equal(transform_log, np.zeros((4, 4)))
A2B2 = transform_from_transform_log(transform_log)
assert_array_almost_equal(A2B, A2B2)
random_state = np.random.RandomState(84)
A2B = transform_from(np.eye(3), p=random_vector(random_state, 3))
transform_log = transform_log_from_transform(A2B)
A2B2 = transform_from_transform_log(transform_log)
assert_array_almost_equal(A2B, A2B2)
for _ in range(5):
A2B = random_transform(random_state)
transform_log = transform_log_from_transform(A2B)
A2B2 = transform_from_transform_log(transform_log)
assert_array_almost_equal(A2B, A2B2)
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()
