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,
