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)