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_screw_parameters_from_dual_quaternion():
dq = np.array([1, 0, 0, 0, 0, 0, 0, 0])
q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq)
assert_array_almost_equal(q, np.zeros(3))
assert_array_almost_equal(s_axis, np.array([1, 0, 0]))
assert_true(np.isinf(h))
assert_almost_equal(theta, 0)
dq = dual_quaternion_from_pq(np.array([1.2, 1.3, 1.4, 1, 0, 0, 0]))
q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq)
assert_array_almost_equal(q, np.zeros(3))
assert_array_almost_equal(s_axis, norm_vector(np.array([1.2, 1.3, 1.4])))
assert_true(np.isinf(h))
assert_almost_equal(theta, np.linalg.norm(np.array([1.2, 1.3, 1.4])))
random_state = np.random.RandomState(1001)
quat = random_quaternion(random_state)
a = axis_angle_from_quaternion(quat)
dq = dual_quaternion_from_pq(np.r_[0, 0, 0, quat])
q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq)
assert_array_almost_equal(q, np.zeros(3))
assert_array_almost_equal(s_axis, a[:3])
assert_array_almost_equal(h, 0)
assert_array_almost_equal(theta, a[3])
for _ in range(5):
A2B = random_transform(random_state)
dq = dual_quaternion_from_transform(A2B)
Stheta = exponential_coordinates_from_transform(A2B)
S, theta = screw_axis_from_exponential_coordinates(Stheta)
q, s_axis, h = screw_parameters_from_screw_axis(S)
q2, s_axis2, h2, theta2 = screw_parameters_from_dual_quaternion(dq)
assert_screw_parameters_equal(q, s_axis, h, theta, q2, s_axis2, h2,
theta2)
def test_check_screw_parameters():
q = [100.0, -20.3, 1e-3]
s_axis = norm_vector(np.array([-1.0, 2.0, 3.0])).tolist()
h = 0.2
assert_raises_regexp(ValueError, "Expected 3D vector with shape",
check_screw_parameters, [0.0], s_axis, h)
assert_raises_regexp(ValueError, "Expected 3D vector with shape",
check_screw_parameters, q, [0.0], h)
assert_raises_regexp(ValueError, "s_axis must not have norm 0",
check_screw_parameters, q, np.zeros(3), h)
q2, s_axis2, h2 = check_screw_parameters(q, 2.0 * np.array(s_axis), h)
assert_array_almost_equal(q, q2)
assert_almost_equal(h, h2)
assert_almost_equal(np.linalg.norm(s_axis2), 1.0)
q2, s_axis2, h2 = check_screw_parameters(q, s_axis, h)
assert_array_almost_equal(q, q2)
assert_array_almost_equal(s_axis, s_axis2)
assert_almost_equal(h, h2)
q2, s_axis2, h2 = check_screw_parameters(q, s_axis, np.inf)
assert_array_almost_equal(np.zeros(3), q2)
assert_array_almost_equal(s_axis, s_axis2)
assert_almost_equal(np.inf, h2)
def test_norm_exponential_coordinates():
Stheta_only_translation = np.array([0.0, 0.0, 0.0, 100.0, 25.0, -23.0])
Stheta_only_translation2 = norm_exponential_coordinates(
Stheta_only_translation)
assert_array_almost_equal(Stheta_only_translation,
Stheta_only_translation2)
random_state = np.random.RandomState(381)
# 180 degree rotation ambiguity
for _ in range(10):
q = random_state.randn(3)
s = norm_vector(random_state.randn(3))
h = random_state.randn()
Stheta = screw_axis_from_screw_parameters(q, s, h) * np.pi
Stheta2 = screw_axis_from_screw_parameters(q, -s, -h) * np.pi
assert_array_almost_equal(
transform_from_exponential_coordinates(Stheta),
transform_from_exponential_coordinates(Stheta2))
assert_array_almost_equal(norm_exponential_coordinates(Stheta),
norm_exponential_coordinates(Stheta2))
for _ in range(10):
Stheta = random_state.randn(6)
# ensure that theta is not within [-pi, pi]
Stheta[random_state.randint(0, 3)] += np.pi + random_state.rand()
Stheta_norm = norm_exponential_coordinates(Stheta)
assert_false(np.all(Stheta == Stheta_norm))
A2B = transform_from_exponential_coordinates(Stheta)
Stheta2 = exponential_coordinates_from_transform(A2B)
assert_array_almost_equal(Stheta2, Stheta_norm)
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_dual_quaternion_from_screw_parameters():
q = np.zeros(3)
s_axis = np.array([1, 0, 0])
h = np.inf
theta = 0
dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta)
assert_array_almost_equal(dq, np.array([1, 0, 0, 0, 0, 0, 0, 0]))
q = np.zeros(3)
s_axis = norm_vector(np.array([2.3, 2.4, 2.5]))
h = np.inf
theta = 3.6
dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta)
pq = pq_from_dual_quaternion(dq)
assert_array_almost_equal(pq, np.r_[s_axis * theta, 1, 0, 0, 0])
q = np.zeros(3)
s_axis = norm_vector(np.array([2.4, 2.5, 2.6]))
h = 0
theta = 4.1
dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta)
pq = pq_from_dual_quaternion(dq)
assert_array_almost_equal(pq[:3], [0, 0, 0])
assert_array_almost_equal(axis_angle_from_quaternion(pq[3:]),
norm_axis_angle(np.r_[s_axis, theta]))
random_state = np.random.RandomState(1001)
for _ in range(5):
A2B = random_transform(random_state)
Stheta = exponential_coordinates_from_transform(A2B)
S, theta = screw_axis_from_exponential_coordinates(Stheta)
q, s_axis, h = screw_parameters_from_screw_axis(S)
dq = dual_quaternion_from_screw_parameters(q, s_axis, h, theta)
assert_unit_dual_quaternion(dq)
dq_expected = dual_quaternion_from_transform(A2B)
assert_unit_dual_quaternion_equal(dq, dq_expected)
def test_assert_screw_parameters_equal():
q = np.array([1, 2, 3])
s_axis = norm_vector(np.array([-2, 3, 5]))
h = 2
theta = 1
assert_screw_parameters_equal(q, s_axis, h, theta, q + 484.3 * s_axis,
s_axis, h, theta)
assert_raises_regexp(AssertionError,
"-1492.7128508189376 != 995.1419005459584",
assert_screw_parameters_equal, q, s_axis, h, theta,
q + 484.3, s_axis, h, theta)
s_axis_mirrored = -s_axis
theta_mirrored = 2.0 * np.pi - theta
h_mirrored = -h * theta / theta_mirrored
assert_screw_parameters_equal(q, s_axis_mirrored, h_mirrored,
theta_mirrored, q, s_axis, h, theta)
[test[1] / 2.0, test[1]])
rot[3].set_3d_properties([test[2] / 2.0, test[2]])
velocity.append(
pr.angle_between_vectors(a[:3], last_a[:3]) + a[3] - last_a[3])
last_a = a
profile.set_data(np.linspace(0, 1, n_frames)[:len(velocity)], velocity)
return rot
if __name__ == "__main__":
# Generate random start and goal
np.random.seed(3)
start = np.array([0, 0, 0, np.pi])
start[:3] = pr.norm_vector(np.random.randn(3))
end = np.array([0, 0, 0, np.pi])
end[:3] = pr.norm_vector(np.random.randn(3))
n_frames = 100
fig = plt.figure(figsize=(12, 5))
ax = fig.add_subplot(121, projection="3d")
ax.set_xlim((-1, 1))
ax.set_ylim((-1, 1))
ax.set_zlim((-1, 1))
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
Rs = pr.matrix_from_axis_angle(start)
