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)