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 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)