def test_conversions_between_exponential_coordinates_and_transform(): A2B = np.eye(4) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) A2B2 = transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B = translate_transform(np.eye(4), [1.0, 5.0, 0.0]) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0]) A2B2 = transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B = rotate_transform(np.eye(4), active_matrix_from_angle(2, 0.5 * np.pi)) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0]) A2B2 = transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) random_state = np.random.RandomState(52) for _ in range(5): A2B = random_transform(random_state) Stheta = exponential_coordinates_from_transform(A2B) A2B2 = transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2)
def test_transforms_from_exponential_coordinates(): A2B = np.eye(4) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) A2B2 = transforms_from_exponential_coordinates([Stheta])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0] assert_array_almost_equal(A2B, A2B2) A2B = translate_transform(np.eye(4), [1.0, 5.0, 0.0]) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0]) A2B2 = transforms_from_exponential_coordinates([Stheta])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0] assert_array_almost_equal(A2B, A2B2) A2B = rotate_transform(np.eye(4), active_matrix_from_angle(2, 0.5 * np.pi)) Stheta = exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0]) A2B2 = transforms_from_exponential_coordinates([Stheta])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0] assert_array_almost_equal(A2B, A2B2) random_state = np.random.RandomState(53) for _ in range(5): A2B = random_transform(random_state) Stheta = exponential_coordinates_from_transform(A2B) A2B2 = transforms_from_exponential_coordinates([Stheta])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = transforms_from_exponential_coordinates([[Stheta], [Stheta]])[0, 0] assert_array_almost_equal(A2B, A2B2)
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)