def test_batch_concatenate_quaternions_1d(): random_state = np.random.RandomState(230) q1 = pr.random_quaternion(random_state) q2 = pr.random_quaternion(random_state) q12 = np.empty(4) pbr.batch_concatenate_quaternions(q1, q2, out=q12) assert_array_almost_equal( q12, pr.concatenate_quaternions(q1, q2))
def test_quaternion_slerp_batch(): random_state = np.random.RandomState(229) q_start = pr.random_quaternion(random_state) q_end = pr.random_quaternion(random_state) t = np.linspace(0, 1, 101) Q = pbr.quaternion_slerp_batch(q_start, q_end, t) for i in range(len(t)): qi = pr.quaternion_slerp(q_start, q_end, t[i]) pr.assert_quaternion_equal(Q[i], qi)
def test_transforms_from_pqs_0dims(): random_state = np.random.RandomState(0) pq = np.empty(7) pq[:3] = random_state.randn(3) pq[3:] = random_quaternion(random_state) A2B = transforms_from_pqs(pq, False) assert_array_almost_equal(A2B, transform_from_pq(pq))
def test_quaternions_from_matrices_no_batch(): random_state = np.random.RandomState(85) for _ in range(5): q = pr.random_quaternion(random_state) R = pr.matrix_from_quaternion(q) q2 = pbr.quaternions_from_matrices(R) pr.assert_quaternion_equal(q, q2) a = np.array([1.0, 0.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 1.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 0.0, 1.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R)
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_png_export(): """Test if the graph can be exported to PNG.""" random_state = np.random.RandomState(0) ee2robot = transform_from_pq( np.hstack((np.array([0.4, -0.3, 0.5]), random_quaternion(random_state)))) cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id))) object2cam = transform_from( active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, 0.5])), np.array([0.5, 0.1, 0.1])) tm = TransformManager() tm.add_transform("end-effector", "robot", ee2robot) tm.add_transform("camera", "robot", cam2robot) tm.add_transform("object", "camera", object2cam) _, filename = tempfile.mkstemp(".png") try: tm.write_png(filename) assert_true(os.path.exists(filename)) except ImportError: raise SkipTest("pydot is required for this test") finally: if os.path.exists(filename): try: os.remove(filename) except WindowsError: pass # workaround for permission problem on Windows
def test_quaternions_from_matrices_4d(): random_state = np.random.RandomState(84) for _ in range(5): q = pr.random_quaternion(random_state) R = pr.matrix_from_quaternion(q) q2 = pbr.quaternions_from_matrices([[R, R], [R, R]]) pr.assert_quaternion_equal(q, q2[0, 0]) pr.assert_quaternion_equal(q, q2[0, 1]) pr.assert_quaternion_equal(q, q2[1, 0]) pr.assert_quaternion_equal(q, q2[1, 1])
def test_matrices_from_pos_quat(): """Test conversion from positions and quaternions to matrices.""" P = np.empty((10, 7)) random_state = np.random.RandomState(0) P[:, :3] = random_state.randn(10, 3) for t in range(10): P[t, 3:] = random_quaternion(random_state) H = matrices_from_pos_quat(P) for t in range(10): assert_array_almost_equal(P[t, :3], H[t, :3, 3]) assert_quaternion_equal(P[t, 3:], quaternion_from_matrix(H[t, :3, :3]))
def test_batch_concatenate_q_conj(): random_state = np.random.RandomState(231) Q = np.array([pr.random_quaternion(random_state) for _ in range(10)]) Q = Q.reshape(2, 5, 4) Q_conj = pbr.batch_q_conj(Q) Q_Q_conj = pbr.batch_concatenate_quaternions(Q, Q_conj) assert_array_almost_equal( Q_Q_conj.reshape(-1, 4), np.array([[1, 0, 0, 0]] * 10))
def test_matrices_from_quaternions(): random_state = np.random.RandomState(83) for _ in range(5): q = pr.random_quaternion(random_state) R = pbr.matrices_from_quaternions([q], normalize_quaternions=False)[0] q2 = pr.quaternion_from_matrix(R) pr.assert_quaternion_equal(q, q2) for _ in range(5): q = random_state.randn(4) R = pbr.matrices_from_quaternions([q], normalize_quaternions=True)[0] q2 = pr.quaternion_from_matrix(R) pr.assert_quaternion_equal(q / np.linalg.norm(q), q2)
def test_transform_point_cloud_trans_quat(self): from pytransform3d.rotations import random_quaternion frame_name = 'test_frame' for cloud_msg in TestCloudProcessing._cloud_messages: for _ in range(TestCloudProcessing._transform_num): translation = np.random.rand(3) * 1.0 # scale to 1 meter rotation = random_quaternion() # quaternion transformed_cloud = transform_point_cloud_trans_quat( cloud_msg, translation, rotation, frame_name) self.assertIs( type(transformed_cloud), PointCloud2, "'transform_point_cloud_trans_quat' does not return type 'sensor_msgs/PointCloud2'" ) self.assertTrue( transformed_cloud.header.frame_id == frame_name)
def test_conversions_between_dual_quternion_and_transform(): random_state = np.random.RandomState(1000) for _ in range(5): A2B = random_transform(random_state) dq = dual_quaternion_from_transform(A2B) A2B2 = transform_from_dual_quaternion(dq) assert_array_almost_equal(A2B, A2B2) dq2 = dual_quaternion_from_transform(A2B2) assert_unit_dual_quaternion_equal(dq, dq2) for _ in range(5): p = random_vector(random_state, 3) q = random_quaternion(random_state) dq = dual_quaternion_from_pq(np.hstack((p, q))) A2B = transform_from_dual_quaternion(dq) dq2 = dual_quaternion_from_transform(A2B) assert_unit_dual_quaternion_equal(dq, dq2) A2B2 = transform_from_dual_quaternion(dq2) assert_array_almost_equal(A2B, A2B2)
def test_quaternion_slerp_batch_zero_angle(): random_state = np.random.RandomState(228) q = pr.random_quaternion(random_state) Q = pbr.quaternion_slerp_batch(q, q, [0.5]) pr.assert_quaternion_equal(q, Q[0])
In this example, we will use the transform manager to infer a transformation automatically. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt from pytransform3d.rotations import (random_quaternion, matrix_from_euler_xyz, q_id) from pytransform3d.transformations import transform_from_pq, transform_from from pytransform3d.transform_manager import TransformManager random_state = np.random.RandomState(0) ee2robot = transform_from_pq( np.hstack((np.array([0.4, -0.3, 0.5]), random_quaternion(random_state)))) cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id))) object2cam = transform_from(matrix_from_euler_xyz(np.array([0.0, 0.0, 0.5])), np.array([0.5, 0.1, 0.1])) tm = TransformManager() tm.add_transform("end-effector", "robot", ee2robot) tm.add_transform("camera", "robot", cam2robot) tm.add_transform("object", "camera", object2cam) ee2object = tm.get_transform("end-effector", "object") ax = tm.plot_frames_in("robot", s=0.1) ax.set_xlim((-0.25, 0.75)) ax.set_ylim((-0.5, 0.5)) ax.set_zlim((0.0, 1.0))
def test_batch_q_conj_1d(): random_state = np.random.RandomState(230) q = pr.random_quaternion(random_state) assert_array_almost_equal(pr.q_conj(q), pbr.batch_q_conj(q))
# Draw new frame rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]]) rot[0].set_3d_properties([0, R[2, 0]]) rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]]) rot[1].set_3d_properties([0, R[2, 1]]) rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]]) rot[2].set_3d_properties([0, R[2, 2]]) return rot if __name__ == "__main__": random_state = np.random.RandomState(3) start = pr.random_quaternion(random_state) n_frames = 1000 dt = 0.01 angular_accelerations = np.empty((n_frames, 3)) for i in range(n_frames): angular_accelerations[i] = pr.random_compact_axis_angle(random_state) # Integrate angular accelerations to velocities angular_velocities = np.vstack((np.zeros( (1, 3)), np.cumsum(angular_accelerations * dt, axis=0))) # Integrate angular velocities to quaternions Q = pr.quaternion_integrate(angular_velocities, q0=start, dt=dt) fig = plt.figure(figsize=(4, 3)) ax = fig.add_subplot(111, projection="3d") ax.set_xlim((-1, 1))