def test_both_axes(self):
point1 = {
'rx': 0,
'rz': 0.523599,
'x': -0.01529,
'y': 0.0506,
'z': 0.01975
}
point2 = {
'rx': 0,
'rz': 1.0472,
'x': -0.01529,
'y': 0.0506,
'z': 0.01975
}
lin_axes = {'x', 'y', 'z'}
rot_axes = {'rx', 'rz'}
pos1 = numpy.array([point1[a] for a in lin_axes])
pos2 = numpy.array([point2[a] for a in lin_axes])
exp_lin_error = scipy.spatial.distance.euclidean(pos1, pos2)
# the rotation difference is 30 degree
expected_rotation = getRotationMatrix("rz", numpy.radians(30))
exp_rot_error = SCALING_FACTOR * numpy.trace(
numpy.eye(3) - expected_rotation)
act_error = _getDistance(point1, point2)
self.assertAlmostEqual(act_error,
exp_rot_error + exp_lin_error,
places=6)
def test_only_rotation_axes_but_whtout_difference(self):
point1 = {'rx': 0, 'rz': 0.523599} # 30 degree
point2 = {'rx': 0, 'rz': 0.523599} # 30 degree
# the rotation difference is 0 degree
exp_rot_error = 0
act_rot_error = _getDistance(point2, point1)
self.assertAlmostEqual(exp_rot_error, act_rot_error)
def test_only_linear_axes(self):
point1 = {'x': 0.023, 'y': 0.032, 'z': 0.01}
point2 = {'x': 0.082, 'y': 0.01, 'z': 0.028}
pos1 = numpy.array([point1[a] for a in list(point1.keys())])
pos2 = numpy.array([point2[a] for a in list(point2.keys())])
expected_distance = scipy.spatial.distance.euclidean(pos1, pos2)
actual_distance = _getDistance(point1, point2)
self.assertAlmostEqual(expected_distance, actual_distance)
def test_only_rotation_axes_but_whtout_common_axes(self):
point1 = {'rx': 0, 'rz': 0.523599} # 30 degree
point2 = {'rz': 1.0472} # 60 degree
# the rotation difference is 30 degree
expected_rotation = getRotationMatrix("rz", numpy.radians(30))
exp_rot_error = SCALING_FACTOR*numpy.trace(numpy.eye(3)-expected_rotation)
act_rot_error = _getDistance(point2, point1)
self.assertAlmostEqual(exp_rot_error, act_rot_error, places=5)
def test_only_linear_axes_but_without_common_axes(self):
point1 = {'x': 0.023, 'y': 0.032}
point2 = {'x': 0.023, 'y': 0.032, 'z': 1}
common_axes = point1.keys() & point2.keys()
pos1 = numpy.array([point1[a] for a in common_axes])
pos2 = numpy.array([point2[a] for a in common_axes])
expected_distance = scipy.spatial.distance.euclidean(pos1, pos2)
actual_distance = _getDistance(point1, point2)
self.assertAlmostEqual(expected_distance, actual_distance)
def test_only_linear_axes_but_without_difference(self):
point1 = {'x': 0.082, 'y': 0.01, 'z': 0.028}
point2 = {'x': 0.082, 'y': 0.01, 'z': 0.028}
expected_distance = 0
actual_distance = _getDistance(point1, point2)
self.assertAlmostEqual(expected_distance, actual_distance)
