def writeTests(config, format): NBSAMPLES = 128 a = [Quaternion.random() for x in range(NBSAMPLES)] b = [Quaternion.random() for x in range(NBSAMPLES)] config.writeInput(1, flattenQuat(a)) config.writeInput(2, flattenQuat(b)) normTest = [x.norm for x in a] config.writeReference(1, normTest) inverseTest = [x.inverse for x in a] config.writeReference(2, flattenQuat(inverseTest)) conjugateTest = [x.conjugate for x in a] config.writeReference(3, flattenQuat(conjugateTest)) normalizeTest = [x.normalised for x in a] config.writeReference(4, flattenQuat(normalizeTest)) multTest = [a[i] * b[i] for i in range(NBSAMPLES)] config.writeReference(5, flattenQuat(multTest)) quat2RotTest = [x.rotation_matrix for x in a] config.writeReference(6, flattenRot(quat2RotTest)) config.writeInput(7, flattenRot(quat2RotTest)) rot2QuatTest = [mkQuaternion(x) for x in quat2RotTest] config.writeReference(7, flattenQuat(rot2QuatTest))
def generate_quaternion(): q1 = Quaternion.random() q2 = Quaternion.random() while True: for q in Quaternion.intermediates(q1, q2, 20, include_endpoints=True): yield q #q1, q2 = q2, q1 q1 = q2 q2 = Quaternion.random()
def test_copy(self): from copy import copy q = Quaternion.random() q2 = copy(q) self.assertEqual(q, q2) self.assertFalse(q is q2) self.assertTrue(all(q.q == q2.q))
def test_differentiation(self): q = Quaternion.random() omega = np.random.uniform(-1, 1, 3) # Random angular velocity q_dash = 0.5 * q * Quaternion(vector=omega) self.assertEqual(q_dash, q.derivative(omega))
def generate_data(num_nodes): channel_data = cd.ChannelData() for __ in range(num_nodes): channel_data.node_list.extend([float(20 * np.random.ranf((1, 1)))]) channel_data.node_list.extend([float(20 * np.random.ranf((1, 1)))]) channel_data.node_list.extend([float(20 * np.random.ranf((1, 1)))]) channel_data.node_list.extend(Quaternion.random()) los = 0 other = 0 for i in range(random.randint(1, 2)): # select pairs of nodes path_details = channel_data.path_details.add() a, b = random.sample((range(1, num_nodes + 1)), k=2) path_details.ids.extend([a, b]) path_details.los = True los += int(path_details.los) for j in range(random.randint(0, 3)): # number of paths for given nodes k = random.randint(1, 3) # number of hops in path path_details.num_hops.append(k) # import pdb; pdb.set_trace() path_details.hop_points.extend( np.random.rand(1, (4 * k)).tolist()[0]) other += 1 return channel_data.SerializeToString()
def getRandomPose(self): """ Generate a random pose return -- geometry_msgs/PoseStamped """ pose = geometry_msgs.msg.PoseStamped() pose.header.frame_id = rosparamOrDefault( '/bin_picking/pose_reference_frame', 'base_link') pose.pose.position.x = random.uniform(self.min_x_object, self.max_x_object) pose.pose.position.y = random.uniform(self.min_y_object, self.max_y_object) pose.pose.position.z = random.uniform(self.min_z_object, self.max_z_object) quat = Quaternion.random() pose.pose.orientation.x = quat[1] pose.pose.orientation.y = quat[2] pose.pose.orientation.z = quat[3] pose.pose.orientation.w = quat[0] return pose
def test_deep_copy(self): from copy import deepcopy q = Quaternion.random() q2 = deepcopy(q) self.assertEqual(q, q2) self.assertFalse(q is q2) self.assertFalse(q.q is q2.q)
def test_tf_inverse(self): # Extensively test the inverse function: for _ in range(0, 100): tf = Transform([random.random(), random.random(), random.random()], Quaternion.random()) self.assertEqual(tf * tf.inverse, Transform([0, 0, 0], 0)) self.assertEqual(tf.inverse * tf, Transform([0, 0, 0], 0))
def test_orientation_repr() -> None: """Test the __repr__ method of the Orientation class.""" o = Orientation(Quaternion.random()) assert repr(o) == f"Orientation(" \ f"rot_x={o.rot_x}, " \ f"rot_y={o.rot_y}, " \ f"rot_z={o.rot_z}" \ f")"
def test_init_copy(self): q1 = Quaternion.random() q2 = Quaternion(q1) self.assertIsInstance(q2, Quaternion) self.assertEqual(q2, q1) with self.assertRaises(TypeError): q3 = Quaternion(None) with self.assertRaises(ValueError): q4 = Quaternion("String")
def test_conjugate(self): a, b, c, d = randomElements() q1 = Quaternion(a, b, c, d) q2 = Quaternion.random() self.assertEqual(q1.conjugate, Quaternion(a, -b, -c, -d)) self.assertEqual((q1 * q2).conjugate, q2.conjugate * q1.conjugate) self.assertEqual((q1 + q1.conjugate) / 2, Quaternion(scalar=q1.scalar)) self.assertEqual((q1 - q1.conjugate) / 2, Quaternion(vector=q1.vector))
def test_inverse(self): q1 = Quaternion(randomElements()) q2 = Quaternion.random() if q1: self.assertEqual(q1 * q1.inverse, Quaternion(1.0, 0.0, 0.0, 0.0)) else: with self.assertRaises(ZeroDivisionError): q1 * q1.inverse self.assertEqual(q2 * q2.inverse, Quaternion(1.0, 0.0, 0.0, 0.0))
def test_norm(self): r = randomElements() q1 = Quaternion(*r) q2 = Quaternion.random() self.assertEqual(q1.norm, np.linalg.norm(np.array(r))) self.assertEqual(q1.magnitude, np.linalg.norm(np.array(r))) # Multiplicative norm self.assertAlmostEqual((q1 * q2).norm, q1.norm * q2.norm, ALMOST_EQUAL_TOLERANCE) # Scaled norm for s in [30.0, 0.3, -2, -4.7]: self.assertAlmostEqual((q1 * s).norm, q1.norm * abs(s), ALMOST_EQUAL_TOLERANCE)
def rotate_point_cloud_rnd(data): """ Randomly rotate the point clouds to augument the dataset rotation is per shape based along random directions Input: Nx3 array, original point clouds Return: Nx3 array, rotated point clouds """ quat = Q.random() quat = quat.rotation_matrix rotated_data = np.matmul(data, quat) return rotated_data
def test_matrix_io(self): v = np.random.uniform(-100, 100, 3) for i in range(10): q0 = Quaternion.random() R = q0.rotation_matrix q1 = Quaternion(matrix=R) np.testing.assert_almost_equal(q0.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE) np.testing.assert_almost_equal(q0.rotate(v), q1.rotate(v), decimal=ALMOST_EQUAL_TOLERANCE) np.testing.assert_almost_equal(q1.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE) self.assertTrue((q0 == q1) or (q0 == -q1)) # q1 and -q1 are equivalent rotations
def reorient(self, method='random', rotations=50): if method == 'IDL': # based on max_agg3.pro from IPAS max_area = 0 current_rot = self.rotation for i in range(rotations): [a, b, c] = [ np.random.uniform(high=np.pi / 4), np.random.uniform(high=np.pi / 4), np.random.uniform(high=np.pi / 4) ] # for mysterious reasons we are going to rotate this 3 times rot1 = self._euler_to_mat([a, b, c]) rot2 = self._euler_to_mat([b * np.pi, c * np.pi, a * np.pi]) rot3 = self._euler_to_mat( [c * np.pi * 2, a * np.pi * 2, b * np.pi * 2]) desired_rot = Quaternion(matrix=rot1 * rot2 * rot3) rot_mat = (desired_rot * current_rot.inverse).rotation_matrix self._rotate_mat(rot_mat) new_area = self.projectxy().area if new_area > max_area: max_area = new_area max_rot = desired_rot # save our spot current_rot = desired_rot # rotate new crystal to the area-maximizing rotation rot_mat = (max_rot * current_rot.inverse).rotation_matrix self._rotate_mat(rot_mat) elif method == 'random': # same as schmitt but only rotating one time, with a real # random rotation max_area = 0 current_rot = self.rotation for i in range(rotations): desired_rot = Quaternion.random() rot_mat = (desired_rot * current_rot.inverse).rotation_matrix self._rotate_mat(rot_mat) new_area = self.projectxy().area if new_area > max_area: max_area = new_area max_rot = desired_rot # save our spot current_rot = desired_rot # rotate new crystal to the area-maximizing rotation rot_mat = (max_rot * current_rot.inverse).rotation_matrix self._rotate_mat(rot_mat) self.rotation = Quaternion()
def pointcloud_generation(config): ''' Create transformed pointcloud according to the config it is given. :param config: Tuple-like: [0]: dimension of the pointcloud [1]: pointcloud generation method [2]: size :return: Transformed pointcloud with its attributes: size, translation, yaw, pitch, roll ''' mean = [0, 0, 0] cov = [[0.015, 0, 0], [0, 0.015, 0], [0, 0, 0.015]] dimension = config[0] cloud = config[1](size=voxelSpaceSize, dimension=dimension) # cloud = generate_cloud_sphere(size=voxelSpaceSize, dimension=dimension) transl = np.random.uniform([-0.6, -0.6, -0.6], [0.6, 0.6, 0.6], (1,3))[0] # transl = np.random.multivariate_normal(mean, cov, 1)[0] quat = Quaternion.random() cloud_transformed = transform_cloud(cloud, quat, cube_position=transl) # # config.append((dimension, quat, transl)) # voxel = cloud2voxel(cloud_transformed, voxelRange, size=voxelSpaceSize) return cloud_transformed, (config[2], transl, *quat.yaw_pitch_roll)
def test_power(self): q1 = Quaternion.random() q2 = Quaternion(q1) self.assertEqual(q1 ** 0, Quaternion()) self.assertEqual(q1 ** 1, q1) q2 **= 4 self.assertEqual(q2, q1 * q1 * q1 * q1) self.assertEqual((q1 ** 0.5) * (q1 ** 0.5), q1) self.assertEqual(q1 ** -1, q1.inverse) self.assertEqual(4 ** Quaternion(2), Quaternion(16)) with self.assertRaises(TypeError): q1 ** None with self.assertRaises(ValueError): q1 ** 's' q3 = Quaternion() self.assertEqual(q3 ** 0.5, q3) # Identity behaves as an identity self.assertEqual(q3 ** 5, q3) self.assertEqual(q3 ** 3.4, q3) q4 = Quaternion(scalar=5) # real number behaves as any other real number would self.assertEqual(q4 ** 4, Quaternion(scalar=5 ** 4))
def test_conversion_to_ypr(self): def R_x(theta): c = cos(theta) s = sin(theta) return np.array([ [1, 0, 0], [0, c,-s], [0, s, c]]) def R_y(theta): c = cos(theta) s = sin(theta) return np.array([ [ c, 0, s], [ 0, 1, 0], [-s, 0, c]]) def R_z(theta): c = cos(theta) s = sin(theta) return np.array([ [ c,-s, 0], [ s, c, 0], [ 0, 0, 1]]) p = np.random.randn(3) q = Quaternion.random() yaw, pitch, roll = q.yaw_pitch_roll p_q = q.rotate(p) R_q = q.rotation_matrix # build rotation matrix, R = R_z(yaw)*R_y(pitch)*R_x(roll) R_ypr = np.dot(R_x(roll), np.dot(R_y(pitch), R_z(yaw))) p_ypr = np.dot(R_ypr, p) np.testing.assert_almost_equal(p_q , p_ypr, decimal=ALMOST_EQUAL_TOLERANCE) np.testing.assert_almost_equal(R_q , R_ypr, decimal=ALMOST_EQUAL_TOLERANCE)
def get_random_state(ground=False): minX = 0 maxX = 8 minY = 0 maxY = 10 minZ = 0.317 maxZ = 3 while True: x = random.uniform(minX, maxX) y = random.uniform(minY, maxY) z = random.uniform(minZ, maxZ) q = Q.random() if ground: z = 0.317 q = tf.transformations.quaternion_from_euler( 0, 0, random.uniform(0, 2 * pi)) q = Q(q.item(0), q.item(1), q.item(2), q.item(3)) state = SE3(x, y, z, q) T, R = state.get_transition_rotation() if not pqp_client(T, R).result: # no collision return state
def test_conversion_to_matrix(self): q = Quaternion.random() a, b, c, d = tuple(q.elements) R = np.array([ [a**2 + b**2 - c**2 - d**2, 2 * (b * c - a * d), 2 * (a * c + b * d)], [2 * (b * c + a * d), a**2 - b**2 + c**2 - d**2, 2 * (c * d - a * b)], [2 * (b * d - a * c), 2 * (a * b + c * d), a**2 - b**2 - c**2 + d**2]]) t = np.array([[0],[0],[0]]) T = np.vstack([np.hstack([R,t]), np.array([0,0,0,1])]) np.testing.assert_almost_equal(R, q.rotation_matrix, decimal=ALMOST_EQUAL_TOLERANCE) np.testing.assert_almost_equal(T, q.transformation_matrix, decimal=ALMOST_EQUAL_TOLERANCE) # Test no scaling of rotated vectors v1 = np.array([1, 0, 0]) v2 = np.hstack((np.random.uniform(-10, 10, 3), 1.0)) v1_ = np.dot(q.rotation_matrix, v1) v2_ = np.dot(q.transformation_matrix, v2) self.assertAlmostEqual(np.linalg.norm(v1_), 1.0, ALMOST_EQUAL_TOLERANCE) self.assertAlmostEqual(np.linalg.norm(v2_), np.linalg.norm(v2), ALMOST_EQUAL_TOLERANCE) # Test transformation of vectors is equivalent for quaternion & matrix np.testing.assert_almost_equal(v1_, q.rotate(v1), decimal=ALMOST_EQUAL_TOLERANCE) np.testing.assert_almost_equal(v2_[0:3], q.rotate(v2[0:3]), decimal=ALMOST_EQUAL_TOLERANCE)
def test_orientation_quaternion() -> None: """Test the quaternion property of the Orientation class.""" q = Quaternion.random() assert q == Orientation(q).quaternion
def set_random_rotation(self): self.quaternion = Quaternion.random() self.rotation_changed = True
def test_orientation_rot_z() -> None: """Test the rot_z property of the Orientation class.""" q = Quaternion.random() assert q.yaw_pitch_roll[2] == Orientation(q).rot_z
def sample_position(position: np.ndarray, n: int): """ Return n random transforms at the given position. """ tf_samples = [Quaternion.random().transformation_matrix for _ in range(n)] for tfi in tf_samples: tfi[:3, 3] = position return tf_samples
def test_orientation_pitch() -> None: """Test the pitch property of the Orientation class.""" q = Quaternion.random() assert q.yaw_pitch_roll[1] == Orientation(q).pitch
def test_orientation_matrix() -> None: """Test the matrix property of the Orientation class.""" q = Quaternion.random() assert allclose(q.rotation_matrix, Orientation(q).rotation_matrix)
def flattenRot(l): return (np.array([list(x) for x in l]).reshape(9 * len(l))) # q and -q are representing the same rotation. # So there is an ambiguity for the tests. # We force the real part of be positive. def mkQuaternion(mat): q = Quaternion(matrix=mat) if q.scalar < 0: return (-q) else: return (q) a = [2.0 * Quaternion.random() for x in range(NBSAMPLES)] src = flattenQuat(a) res = flattenQuat([x.normalised for x in a]) print(res) output = dsp.arm_quaternion_normalize_f32(src) print(output) print("") res = flattenQuat([x.conjugate for x in a]) print(res) output = dsp.arm_quaternion_conjugate_f32(src) print(output) print("") res = flattenQuat([x.inverse for x in a])
def make_random_quaternion(): q_wxyz = np.float32(Quaternion.random().elements) return q_wxyz
trg_normalize_cloud_size = True numberOfShapes = 36 # 10000 # rotation of the points initialized in the 3D-voxel-cube may bring some of these points (e.g. vertices) outside of the 3D-voxel-cube # to ensure all points are in the 3D-voxel-cube after a rotation, either set trg_normalize_cloud_size = True or set the ratio dimension/dimension <= ~0.6 voxelSpaceSize = 16 voxelRange = 2.0 dimension = 1.0 cloud = generate_cloud_cube(size=voxelSpaceSize, dimension=dimension) quaternions = [] cloudShapes = [] voxelShapes = [] for i in range(numberOfShapes): quat = Quaternion.random() angles = np.linspace(0, 2, numberOfShapes) quat = Quaternion(axis=[1, 1, 1], angle=np.pi * angles[i]) if i == 0: quat = Quaternion(axis=[1, 1, 1], angle=np.pi / 4) # if i == 0: quat = Quaternion() # first quat = 1 + 0i + 0j + 0k if i % 100 == 0: print(strftime("%H%M%S", gmtime()), '\t', i, '\t', quat) if trg_normalize_cloud_size: cloud_transformed = normaliza(transform_cloud(cloud, quat), voxelRange) else: cloud_transformed = transform_cloud(cloud, quat) voxels = cloud2voxel(cloud_transformed, voxelRange,