def testQuaternionVectorPair(self):
qv = QuaternionVectorPair.identity()
matrix = Matrix44([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
np.testing.assert_array_almost_equal(qv.toMatrix(), matrix, decimal=5)
matrix = Matrix44([[0, 0, 1, -1], [1, 0, 0, 3], [0, 1, 0, -8],
[0.0, 0.0, 0.0, 1.0]])
qv = QuaternionVectorPair.fromMatrix(matrix)
np.testing.assert_array_almost_equal(qv.vector,
matrix[0:3, 3],
decimal=5)
np.testing.assert_array_almost_equal(qv.quaternion,
[0.5, 0.5, 0.5, 0.5],
decimal=5)
np.testing.assert_array_almost_equal(qv.toMatrix(), matrix, decimal=5)
qv_1 = qv.inverse() * qv
np.testing.assert_array_almost_equal(qv_1.toMatrix(),
np.identity(4),
decimal=5)
qv *= qv.inverse()
np.testing.assert_array_almost_equal(qv.toMatrix(),
np.identity(4),
decimal=5)
self.assertRaises(ValueError, lambda: qv * [0, 1, 2])
self.assertNotEqual(repr(qv), str(qv))
def testMatrix44(self):
m = Matrix44()
np.testing.assert_array_almost_equal(m, np.zeros((4, 4)), decimal=5)
m = Matrix44.identity()
np.testing.assert_array_almost_equal(m, np.eye(4), decimal=5)
m = Matrix44.ones()
np.testing.assert_array_almost_equal(m, np.ones((4, 4)), decimal=5)
m.m13 = 5
np.testing.assert_array_almost_equal(m.r1, [1, 1, 5, 1], decimal=5)
m.r2 = [8, 9, -7, -3]
np.testing.assert_array_almost_equal(m.c3, [5, -7, 1, 1], decimal=5)
m = Matrix44.fromTranslation([2, 3, 4])
expected = [[1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 1, 4], [0, 0, 0, 1]]
np.testing.assert_array_almost_equal(m, expected, decimal=5)
m = Matrix44.ones() + Matrix44.identity()
expected = [[2, 1, 1, 1], [1, 2, 1, 1], [1, 1, 2, 1], [1, 1, 1, 2]]
np.testing.assert_array_almost_equal(m, expected, decimal=5)
m = np.ones((4, 4)) + Matrix44.identity()
expected = [[2, 1, 1, 1], [1, 2, 1, 1], [1, 1, 2, 1], [1, 1, 1, 2]]
np.testing.assert_array_almost_equal(m, expected, decimal=5)
m = Matrix44.ones() - Matrix44.identity()
expected = [[0, 1, 1, 1], [1, 0, 1, 1], [1, 1, 0, 1], [1, 1, 1, 0]]
np.testing.assert_array_almost_equal(m, expected, decimal=5)
m = np.ones((4, 4)) - Matrix44.identity()
expected = [[0, 1, 1, 1], [1, 0, 1, 1], [1, 1, 0, 1], [1, 1, 1, 0]]
np.testing.assert_array_almost_equal(m, expected, decimal=5)
def executeButtonClicked(self):
if self.final_plane_normal is None or self.initial_plane is None:
return
matrix = Matrix44.identity()
matrix[0:3, 0:3] = rotation_btw_vectors(self.initial_plane.normal,
self.final_plane_normal)
if not is_close(matrix, Matrix44.identity()):
self.parent.presenter.transformSample(matrix, self.selected_sample,
TransformType.Custom)
self.clearPicks()
def testMatrix(self):
self.assertRaises(ValueError, Matrix, 2, -1)
self.assertRaises(ValueError, Matrix, 0, 2)
self.assertRaises(ValueError, Matrix, 2, 2, {"values": [[1, 2], [3]]})
m = Matrix(2, 2, dtype=int)
self.assertEqual(m[0, 0], 0)
m[1, 1] = 5
self.assertEqual(m[1, 1], 5)
m = Matrix(2, 2, [[1.0, 2.0], [3.0, 4.0]])
np.testing.assert_array_almost_equal(m.inverse(),
[[-2.0, 1.0], [1.5, -0.5]],
decimal=5)
m1 = m.inverse() @ m # matrix multiplication
np.testing.assert_array_almost_equal(m1, [[1, 0], [0, 1]])
m1 = np.array(m.inverse()) @ m
np.testing.assert_array_almost_equal(m1, [[1, 0], [0, 1]])
m1 = m.inverse() @ np.array(m)
np.testing.assert_array_almost_equal(m1, [[1, 0], [0, 1]])
a = np.array([[1, 2], [3, 4]])
m1 = m * a # element wise multiplication
np.testing.assert_array_almost_equal(m1, [[1, 4], [9, 16]], decimal=5)
m1 = m * 2
np.testing.assert_array_almost_equal(m1, [[2.0, 4.0], [6.0, 8.0]],
decimal=5)
m2 = 2 * m
np.testing.assert_array_almost_equal(m1, m2, decimal=5)
m1 = m + 2
np.testing.assert_array_almost_equal(m1, [[3.0, 4.0], [5.0, 6.0]],
decimal=5)
m1 = m - 2
np.testing.assert_array_almost_equal(m1, [[-1.0, 0.0], [1.0, 2.0]],
decimal=5)
m = Matrix.create(2, 4, [[1, 2, 3, 4], [5, 6, 7, 8]])
v = Vector.create(4, [1, 2, 3, 4])
result = m @ v # matrix vector multiplication
np.testing.assert_array_almost_equal(m.transpose(),
[[1, 5], [2, 6], [3, 7], [4, 8]])
self.assertTrue(isinstance(result, Vector))
np.testing.assert_array_equal(result, [30, 70])
self.assertRaises(ValueError, lambda: Matrix33() + Matrix44())
self.assertRaises(ValueError, lambda: Matrix33() - Matrix44())
self.assertRaises(ValueError, lambda: Matrix33() * Matrix44())
self.assertRaises(ValueError, lambda: Matrix33() * Vector4())
self.assertRaises(ValueError, lambda: Matrix33() @ Matrix44())
self.assertRaises(ValueError, lambda: Matrix33() @ Vector4())
def testManager(self):
manager = CollisionManager(5)
self.assertEqual(manager.max_size, 5)
geometry = [create_cuboid(), create_cuboid()]
transform = [
Matrix44.identity(),
Matrix44.fromTranslation([0, 0, 0.5])
]
manager.addColliders(geometry, transform, movable=True)
self.assertEqual(len(manager.queries), 2)
self.assertEqual(len(manager.colliders), 2)
manager.addColliders([create_cuboid()],
[Matrix44.fromTranslation([0, 0, 2.0])])
self.assertEqual(len(manager.queries), 2)
self.assertEqual(len(manager.colliders), 3)
manager.createAABBSets()
self.assertListEqual(manager.collide(), [True, True, False])
manager.clear()
self.assertEqual(len(manager.queries), 0)
self.assertEqual(len(manager.colliders), 0)
geometry = [create_cuboid(), create_cuboid(), create_cuboid()]
transform = [
Matrix44.identity(),
Matrix44.fromTranslation([0, 0, 0.5]),
Matrix44.fromTranslation([0, 0, 1.5])
]
manager.addColliders(geometry,
transform,
CollisionManager.Exclude.Consecutive,
movable=True)
manager.createAABBSets()
self.assertListEqual(manager.collide(), [False, False, False])
manager.clear()
transform = [
Matrix44.identity(),
Matrix44.identity(),
Matrix44.identity()
]
manager.addColliders(geometry,
transform,
CollisionManager.Exclude.All,
movable=True)
manager.createAABBSets()
self.assertListEqual(manager.collide(), [False, False, False])
def __init__(self, matrix, sample_key, presenter):
super().__init__()
self.matrix = Matrix44(matrix)
self.key = sample_key
self.model = presenter.model
self.setText(f'Transform Sample ({self.key})')
def createPositioningStack():
y_axis = create_cuboid(200, 10, 200)
z_axis = create_cylinder(25, 50)
q1 = Link("Z", [0.0, 0.0, 1.0], [0.0, 0.0, 0.0], Link.Type.Revolute,
-3.14, 3.14, 0, z_axis)
q2 = Link("Y", [0.0, 1.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic,
-200.0, 200.0, 0, y_axis)
s = SerialManipulator("", [q1, q2],
custom_order=[1, 0],
base=Matrix44.fromTranslation([0.0, 0.0, 50.0]))
return PositioningStack(s.name, s)
def testMatrixFromPose(self):
np.testing.assert_array_almost_equal(matrix_from_pose(
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
Matrix44.identity(),
decimal=5)
expected = Matrix44([[0.0, 0.0, 1.0, -2.0], [0.0, 1.0, 0.0, 5.0],
[-1.0, 0.0, 0.0, 11.0], [0.0, 0.0, 0.0, 1.0]])
matrix = matrix_from_pose([-2.0, 5.0, 11.0, 0.0, 90.0, 0.0])
np.testing.assert_array_almost_equal(matrix, expected, decimal=5)
matrix = matrix_from_pose([-2.0, 5.0, 11.0, 0.0, np.pi / 2, 0.0],
angles_in_degrees=False)
np.testing.assert_array_almost_equal(matrix, expected, decimal=5)
matrix = matrix_from_pose([-2.0, 5.0, 11.0, 0.0, np.pi / 2, 0.0],
angles_in_degrees=False,
order="zyx")
np.testing.assert_array_almost_equal(matrix, expected, decimal=5)
expected = Matrix44([
[0.0, -0.707107, 0.707107, 12.0],
[0.707107, 0.5, 0.5, 50.0],
[-0.707107, 0.5, 0.5, -3.0],
[0.0, 0.0, 0.0, 1.0],
])
matrix = matrix_from_pose([12.0, 50.0, -3.0, -45.0, 45.0, 90.0])
np.testing.assert_array_almost_equal(matrix, expected, decimal=5)
expected = Matrix44([
[0.5, 0.5, -0.7071068, 12.0],
[-0.5, -0.5, -0.7071068, 50.0],
[-0.7071068, 0.7071068, 0.0, -3.0],
[0.0, 0.0, 0.0, 1.0],
])
matrix = matrix_from_pose([12.0, 50.0, -3.0, -45.0, 45.0, 90.0],
order="zyx")
np.testing.assert_array_almost_equal(matrix, expected, decimal=5)
self.assertRaises(ValueError, matrix_from_pose, np.zeros(6), True,
"zzz")
def testSimulationWithVectorAlignment(self):
self.points = np.rec.array([([0.0, -100.0, 0.0], True),
([0.0, 100.0, 0.0], True)],
dtype=POINT_DTYPE)
self.vectors = np.zeros((2, 6, 2), dtype=np.float32)
alignment = Matrix44([[0.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
self.vectors[:, 0:3, 1] = alignment[:3, :3].transpose() @ np.array(
self.mock_instrument.q_vectors[0])
self.vectors[:, 3:6, 1] = alignment[:3, :3].transpose() @ np.array(
self.mock_instrument.q_vectors[1])
simulation = Simulation(self.mock_instrument, self.sample, self.points,
self.vectors, alignment)
simulation.args["align_first_order"] = True
simulation.execute(simulation.args)
simulation.process = self.mock_process
simulation.checkResult()
results = [(0.0, 100.0), (0, 100.0), (0, -100.0), (0, -100.0)]
for exp, result in zip(results, simulation.results):
self.assertTrue(result.ik.orientation_converged)
self.assertTrue(result.ik.position_converged)
np.testing.assert_array_almost_equal(exp, result.ik.q, decimal=2)
simulation.results.clear()
self.mock_instrument.positioning_stack.fixed.resetOffsets()
simulation.args["align_first_order"] = False
simulation.execute(simulation.args)
simulation.checkResult()
results = [(0.0, 100.0), (0.0, -100.0), (0, 100.0), (0, -100.0)]
for exp, result in zip(results, simulation.results):
self.assertTrue(result.ik.orientation_converged)
self.assertTrue(result.ik.position_converged)
np.testing.assert_array_almost_equal(exp, result.ik.q, decimal=2)
simulation = Simulation(self.mock_instrument, self.sample, self.points,
self.vectors, alignment)
self.mock_instrument.positioning_stack.fixed.resetOffsets()
simulation.args["align_first_order"] = False
simulation.args["skip_zero_vectors"] = True
simulation.execute(simulation.args)
simulation.process = self.mock_process
simulation.checkResult()
for result in simulation.results[:2]:
self.assertTrue(result.skipped)
self.assertEqual(result.note, "The measurement vector is unset")
results = [(0, 100.0), (0, -100.0)]
for exp, result in zip(results, simulation.results[2:]):
self.assertTrue(result.ik.orientation_converged)
self.assertTrue(result.ik.position_converged)
np.testing.assert_array_almost_equal(exp, result.ik.q, decimal=2)
def testStackToString(self):
q1 = Link("X", [1.0, 0.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic,
-180.0, 120.0, -10)
first = SerialManipulator("first", [q1])
y_axis = create_cuboid(200, 10, 200)
z_axis = create_cylinder(25, 50)
q1 = Link("Z", [0.0, 0.0, 1.0], [0.0, 0.0, 0.0], Link.Type.Revolute,
-3.14, 3.14, 1.57, z_axis)
q2 = Link("Y", [0.0, 1.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic,
-200.0, 200.0, 0, y_axis)
second = SerialManipulator("second", [q1, q2],
custom_order=[1, 0],
base=Matrix44.fromTranslation(
[0.0, 0.0, 50.0]))
stack = PositioningStack("New Stack", first)
stack.addPositioner(second)
stack.links[0].ignore_limits = True
stack.links[1].locked = True
stack_string = stack_to_string(stack)
new_stack = stack_from_string(stack_string)
for l1, l2 in zip(stack.links, new_stack.links):
self.assertEqual(l1.ignore_limits, l2.ignore_limits)
self.assertEqual(l1.locked, l2.locked)
self.assertEqual(l1.type, l2.type)
self.assertEqual(l1.upper_limit, l2.upper_limit)
self.assertEqual(l1.lower_limit, l2.lower_limit)
np.testing.assert_array_almost_equal(l1.home, l2.home)
np.testing.assert_array_almost_equal(l1.joint_axis, l2.joint_axis)
np.testing.assert_array_almost_equal(new_stack.set_points,
stack.set_points)
np.testing.assert_array_almost_equal(new_stack.fixed.base,
stack.fixed.base)
np.testing.assert_array_almost_equal(new_stack.fixed.tool,
stack.fixed.tool)
np.testing.assert_array_almost_equal(new_stack.fixed.order,
stack.fixed.order)
np.testing.assert_array_almost_equal(new_stack.auxiliary[0].base,
stack.auxiliary[0].base)
np.testing.assert_array_almost_equal(new_stack.auxiliary[0].tool,
stack.auxiliary[0].tool)
np.testing.assert_array_almost_equal(new_stack.auxiliary[0].order,
stack.auxiliary[0].order)
def __init__(self, sample, parent):
super().__init__()
self.parent = parent
self.main_layout = QtWidgets.QVBoxLayout()
self.main_layout.setContentsMargins(0, 0, 0, 0)
self.matrix = Matrix44.identity()
self.table_widget = QtWidgets.QTableWidget(4, 4)
self.table_widget.setFixedHeight(120)
self.table_widget.setShowGrid(False)
self.table_widget.setSelectionMode(
QtWidgets.QAbstractItemView.NoSelection)
self.table_widget.setEditTriggers(
QtWidgets.QAbstractItemView.NoEditTriggers)
self.table_widget.verticalHeader().setVisible(False)
self.table_widget.horizontalHeader().setVisible(False)
self.table_widget.verticalHeader().setSectionResizeMode(
QtWidgets.QHeaderView.Stretch)
self.table_widget.horizontalHeader().setSectionResizeMode(
QtWidgets.QHeaderView.Stretch)
self.main_layout.addWidget(self.table_widget)
self.main_layout.addSpacing(10)
self.updateTable()
self.invert_checkbox = QtWidgets.QCheckBox(
'Invert Transformation Matrix')
self.main_layout.addWidget(self.invert_checkbox)
self.main_layout.addSpacing(10)
button_layout = QtWidgets.QHBoxLayout()
self.load_matrix = QtWidgets.QPushButton('Load Matrix')
self.load_matrix.clicked.connect(self.loadMatrix)
self.execute_button = QtWidgets.QPushButton('Apply Transform')
self.execute_button.clicked.connect(self.executeButtonClicked)
button_layout.addWidget(self.load_matrix)
button_layout.addWidget(self.execute_button)
button_layout.addStretch(1)
self.main_layout.addLayout(button_layout)
self.main_layout.addStretch(1)
self.setLayout(self.main_layout)
self.selected_sample = sample
def translate(self, offset):
"""Translates sample, fiducials, measurements and alignment matrix if present
:param offset: X, Y, and Z axis offsets
:type offset: List[float]
"""
if self.key is not None:
mesh = self.model.sample[self.key]
mesh.translate(offset)
else:
for key in self.model.sample.keys():
mesh = self.model.sample[key]
mesh.translate(offset)
self.model.fiducials.points = self.model.fiducials.points + offset
self.model.measurement_points.points = self.model.measurement_points.points + offset
if self.model.alignment is not None:
self.model.alignment = self.model.alignment @ Matrix44.fromTranslation(-offset)
self.model.notifyChange(Attributes.Fiducials)
self.model.notifyChange(Attributes.Measurements)
self.model.notifyChange(Attributes.Vectors)
self.model.notifyChange(Attributes.Sample)
def testHDFReadWrite(self, visual_fn, setting_cls):
visual_fn.return_value = Mesh(
np.array([[0, 0, 0], [0, 1, 0], [0, 1, 1]]),
np.array([0, 1, 2]),
np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]),
)
filename = self.writeTestFile("instrument.json", SAMPLE_IDF)
instrument = read_instrument_description_file(filename)
data = {
"name":
"Test Project",
"instrument":
instrument,
"instrument_version":
"1.0",
"sample": {},
"fiducials":
np.recarray((0, ), dtype=[("points", "f4", 3), ("enabled", "?")]),
"measurement_points":
np.recarray((0, ), dtype=[("points", "f4", 3), ("enabled", "?")]),
"measurement_vectors":
np.empty((0, 3, 1), dtype=np.float32),
"alignment":
None,
}
filename = os.path.join(self.test_dir, "test.h5")
writer.write_project_hdf(data, filename)
result, instrument = reader.read_project_hdf(filename)
self.assertEqual(__version__, result["version"])
self.assertEqual(data["instrument_version"],
result["instrument_version"])
self.assertEqual(data["name"], result["name"],
"Save and Load data are not Equal")
self.assertEqual(data["instrument"].name, result["instrument"],
"Save and Load data are not Equal")
self.assertDictEqual(result["sample"], {})
self.assertTrue(result["fiducials"][0].size == 0
and result["fiducials"][1].size == 0)
self.assertTrue(result["measurement_points"][0].size == 0
and result["measurement_points"][1].size == 0)
self.assertTrue(result["measurement_vectors"].size == 0)
self.assertIsNone(result["alignment"])
self.assertEqual(result["settings"], {})
sample_key = "a mesh"
vertices = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0]])
normals = np.array([[0, 0, 1], [0, 0, 1], [0, 0, 1]])
indices = np.array([0, 1, 2])
mesh_to_write = Mesh(vertices, indices, normals)
fiducials = np.rec.array(
[([11.0, 12.0, 13.0], False), ([14.0, 15.0, 16.0], True),
([17.0, 18.0, 19.0], False)],
dtype=[("points", "f4", 3), ("enabled", "?")],
)
points = np.rec.array(
[([1.0, 2.0, 3.0], True), ([4.0, 5.0, 6.0], False),
([7.0, 8.0, 9.0], True)],
dtype=[("points", "f4", 3), ("enabled", "?")],
)
vectors = np.zeros((3, 3, 2))
vectors[:, :, 0] = [
[0.0000076, 1.0000000, 0.0000480],
[0.0401899, 0.9659270, 0.2556752],
[0.1506346, 0.2589932, 0.9540607],
]
vectors[:, :, 1] = [
[0.1553215, -0.0000486, 0.9878640],
[0.1499936, -0.2588147, 0.9542100],
[0.0403915, -0.9658791, 0.2558241],
]
base = Matrix44(np.random.random((4, 4)))
stack_name = "Positioning Table + Huber Circle"
new_collimator = "Snout 100mm"
jaw_aperture = [7.0, 5.0]
data = {
"name": "demo",
"instrument": instrument,
"instrument_version": "1.1",
"sample": {
sample_key: mesh_to_write
},
"fiducials": fiducials,
"measurement_points": points,
"measurement_vectors": vectors,
"alignment": np.identity(4),
}
instrument.loadPositioningStack(stack_name)
instrument.positioning_stack.fkine([200.0, 0.0, 0.0, np.pi, 0.0])
instrument.positioning_stack.links[0].ignore_limits = True
instrument.positioning_stack.links[4].locked = True
aux = instrument.positioning_stack.auxiliary[0]
instrument.positioning_stack.changeBaseMatrix(aux, base)
instrument.jaws.aperture = jaw_aperture
instrument.jaws.positioner.fkine([-600.0])
instrument.jaws.positioner.links[0].ignore_limits = True
instrument.jaws.positioner.links[0].locked = True
instrument.detectors["Detector"].current_collimator = new_collimator
instrument.detectors["Detector"].positioner.fkine([np.pi / 2, 100.0])
instrument.detectors["Detector"].positioner.links[
0].ignore_limits = True
instrument.detectors["Detector"].positioner.links[1].locked = True
setting_cls.local = {"num": 1, "str": "string", "colour": (1, 1, 1, 1)}
writer.write_project_hdf(data, filename)
result, instrument2 = reader.read_project_hdf(filename)
self.assertEqual(__version__, result["version"])
self.assertEqual(data["name"], result["name"],
"Save and Load data are not Equal")
self.assertEqual(data["instrument_version"],
result["instrument_version"])
self.assertEqual(data["instrument"].name, result["instrument"],
"Save and Load data are not Equal")
self.assertTrue(sample_key in result["sample"])
np.testing.assert_array_almost_equal(fiducials.points,
result["fiducials"][0],
decimal=5)
np.testing.assert_array_almost_equal(points.points,
result["measurement_points"][0],
decimal=5)
np.testing.assert_array_almost_equal(
result["sample"][sample_key].vertices, vertices, decimal=5)
np.testing.assert_array_almost_equal(
result["sample"][sample_key].indices, indices, decimal=5)
np.testing.assert_array_almost_equal(
result["sample"][sample_key].normals, normals, decimal=5)
np.testing.assert_array_almost_equal(fiducials.points,
result["fiducials"][0],
decimal=5)
np.testing.assert_array_almost_equal(points.points,
result["measurement_points"][0],
decimal=5)
np.testing.assert_array_equal(fiducials.enabled,
result["fiducials"][1])
np.testing.assert_array_equal(points.enabled,
result["measurement_points"][1])
np.testing.assert_array_almost_equal(vectors,
result["measurement_vectors"],
decimal=5)
np.testing.assert_array_almost_equal(result["alignment"],
np.identity(4),
decimal=5)
setting = result["settings"]
self.assertEqual(setting["num"], 1)
self.assertEqual(setting["str"], "string")
self.assertEqual(tuple(setting["colour"]), (1, 1, 1, 1))
self.assertEqual(instrument.positioning_stack.name,
instrument2.positioning_stack.name)
np.testing.assert_array_almost_equal(
instrument.positioning_stack.configuration,
instrument2.positioning_stack.configuration,
decimal=5)
for link1, link2 in zip(instrument.positioning_stack.links,
instrument2.positioning_stack.links):
self.assertEqual(link1.ignore_limits, link2.ignore_limits)
self.assertEqual(link1.locked, link2.locked)
for aux1, aux2 in zip(instrument.positioning_stack.auxiliary,
instrument2.positioning_stack.auxiliary):
np.testing.assert_array_almost_equal(aux1.base,
aux2.base,
decimal=5)
np.testing.assert_array_almost_equal(instrument.jaws.aperture,
instrument2.jaws.aperture,
decimal=5)
np.testing.assert_array_almost_equal(
instrument.jaws.aperture_lower_limit,
instrument2.jaws.aperture_lower_limit,
decimal=5)
np.testing.assert_array_almost_equal(
instrument.jaws.aperture_upper_limit,
instrument2.jaws.aperture_upper_limit,
decimal=5)
np.testing.assert_array_almost_equal(
instrument.jaws.positioner.configuration,
instrument2.jaws.positioner.configuration,
decimal=5)
for link1, link2 in zip(instrument.jaws.positioner.links,
instrument2.jaws.positioner.links):
self.assertEqual(link1.ignore_limits, link2.ignore_limits)
self.assertEqual(link1.locked, link2.locked)
detector1 = instrument.detectors["Detector"]
detector2 = instrument2.detectors["Detector"]
self.assertEqual(detector1.current_collimator.name,
detector2.current_collimator.name)
np.testing.assert_array_almost_equal(
detector1.positioner.configuration,
detector2.positioner.configuration,
decimal=5)
for link1, link2 in zip(detector1.positioner.links,
detector2.positioner.links):
self.assertEqual(link1.ignore_limits, link2.ignore_limits)
self.assertEqual(link1.locked, link2.locked)
data["measurement_vectors"] = np.ones((3, 3, 2)) # invalid normals
writer.write_project_hdf(data, filename)
self.assertRaises(ValueError, reader.read_project_hdf, filename)
data["measurement_vectors"] = np.ones(
(3, 6, 2)) # more vector than detectors
writer.write_project_hdf(data, filename)
self.assertRaises(ValueError, reader.read_project_hdf, filename)
data["measurement_vectors"] = np.ones(
(4, 3, 2)) # more vectors than points
writer.write_project_hdf(data, filename)
self.assertRaises(ValueError, reader.read_project_hdf, filename)
def testPositioningStack(self):
q1 = Link("", [0.0, 1.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic,
-3.14, 3.14, 0)
q2 = Link("", [1.0, 0.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic,
-3.14, 3.14, 0)
q3 = Link("", [0.0, 0.0, 1.0], [1.0, 0.0, 0.0], Link.Type.Revolute,
-3.14, 3.14, 0)
q4 = Link("", [0.0, 0.0, 1.0], [1.0, 0.0, 0.0], Link.Type.Revolute,
-3.14, 3.14, 0)
s1 = SerialManipulator("", [q1, q2], custom_order=[1, 0])
s2 = SerialManipulator("", [q3, q4])
ps = PositioningStack(s1.name, s1)
ps.addPositioner(s2)
self.assertListEqual(ps.order, [1, 0, 2, 3])
np.testing.assert_array_almost_equal(ps.toUserFormat(
[0, 1, np.pi / 2, -np.pi / 2]), [1, 0, 90, -90],
decimal=5)
np.testing.assert_array_almost_equal(ps.fromUserFormat([1, 0, 90,
-90]),
[0, 1, np.pi / 2, -np.pi / 2],
decimal=5)
np.testing.assert_array_almost_equal(list(zip(*ps.bounds))[0],
[-3.14] * 4,
decimal=5)
np.testing.assert_array_almost_equal(list(zip(*ps.bounds))[1],
[3.14] * 4,
decimal=5)
self.assertEqual(ps.link_count, 4)
np.testing.assert_array_almost_equal(ps.configuration,
[0.0, 0.0, 0.0, 0.0],
decimal=5)
ps.fkine([100, -50, np.pi / 2, np.pi / 2])
np.testing.assert_array_almost_equal(ps.configuration,
[100, -50, np.pi / 2, np.pi / 2],
decimal=5)
expected_result = [[-1, 0, 0, -51], [0, -1, 0, 101], [0, 0, 1, 0],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(ps.pose,
expected_result,
decimal=5)
self.assertEqual(ps.model().meshes, [])
self.assertEqual(ps.model().transforms, [])
ps = PositioningStack(s1.name, s1)
ps.addPositioner(copy.deepcopy(s1))
ps.addPositioner(copy.deepcopy(s1))
self.assertEqual(ps.link_count, 6)
ps.fkine([100, -50, 20, 30, 45, 32])
expected_result = [[1, 0, 0, 12], [0, 1, 0, 165], [0, 0, 1, 0],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(ps.pose,
expected_result,
decimal=5)
ps.changeBaseMatrix(ps.auxiliary[0],
Matrix44.fromTranslation([0, 0, 5.4]))
ps.fkine([100, -50, 20, 30, 45, 32])
expected_result = [[1, 0, 0, 12], [0, 1, 0, 165], [0, 0, 1, 0],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(ps.pose,
expected_result,
decimal=5)
def testSerialLink(self):
with self.assertRaises(ValueError):
# zero vector as Axis
Link("", [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic, 0,
0, 0)
link_1 = Link("", [0.0, 0.0, 1.0], [0.0, 0.0, 0.0],
Link.Type.Prismatic, 0.0, 600.0, 0.0)
np.testing.assert_array_almost_equal(np.identity(4),
link_1.transformation_matrix,
decimal=5)
link_1.move(200)
expected_result = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 200],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(expected_result,
link_1.transformation_matrix,
decimal=5)
link_2 = Link("", [0.0, 0.0, 1.0], [0.0, 0.0, 0.0], Link.Type.Revolute,
-np.pi, np.pi, np.pi / 2)
expected_result = [[0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(expected_result,
link_2.transformation_matrix,
decimal=5)
link_2.move(0)
np.testing.assert_array_almost_equal(np.identity(4),
link_2.transformation_matrix,
decimal=5)
link_2.reset()
np.testing.assert_array_almost_equal(expected_result,
link_2.transformation_matrix,
decimal=5)
qv = link_2.quaternion_vector_pair
np.testing.assert_array_almost_equal(qv.quaternion,
[0.0, 0.0, 0.70711, 0.70711],
decimal=5)
np.testing.assert_array_almost_equal(qv.vector, [0.0, 0.0, 0.0],
decimal=5)
q1 = Link("", [0.0, 0.0, 1.0], [0.0, 0.0, 0.0], Link.Type.Prismatic, 0,
600, 0)
q2 = Link("", [0.0, 0.0, 1.0], [0.0, 0.0, 0.0], Link.Type.Revolute,
-3.14, 3.14, 0)
q3 = Link("", [0.0, 1.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic,
-250, 250, 0)
q4 = Link("", [1.0, 0.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic,
-250, 250, 0)
s = SerialManipulator("", [q1, q2, q3, q4])
pose_0 = s.fkine([250, 1.57, 20, 30])
np.testing.assert_array_almost_equal(s.configuration,
[250, 1.57, 20, 30],
decimal=5)
s.reset()
np.testing.assert_array_almost_equal(s.configuration, [0, 0, 0, 0],
decimal=5)
np.testing.assert_array_almost_equal(np.identity(4), s.pose, decimal=5)
self.assertEqual(s.link_count, 4)
self.assertEqual(len(s.links), 4)
model = s.model() # should be empty since no mesh is provided
self.assertEqual(model.meshes, [])
self.assertEqual(model.transforms, [])
expected_result = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 250],
[0, 0, 0, 1]]
pose = s.fkine([250, 1.57, 20, 30], end_index=1)
np.testing.assert_array_almost_equal(expected_result, pose, decimal=5)
expected_result = [[1, 0, 0, 30], [0, 1, 0, 20], [0, 0, 1, 0],
[0, 0, 0, 1]]
pose = s.fkine([250, 1.57, 20, 30], start_index=2)
np.testing.assert_array_almost_equal(expected_result, pose, decimal=5)
base = Matrix44([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -500],
[0, 0, 0, 1]])
s.base = base
pose = s.fkine([250, 1.57, 20, 30], start_index=2)
np.testing.assert_array_almost_equal(expected_result, pose, decimal=5)
pose = s.fkine([250, 1.57, 20, 30], end_index=1)
expected_result = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -250],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(expected_result, pose, decimal=5)
pose = s.fkine([250, 1.57, 20, 30], end_index=1, include_base=False)
expected_result = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 250],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(expected_result, pose, decimal=5)
s.tool = base
pose = s.fkine([250, 1.57, 20, 30])
np.testing.assert_array_almost_equal(pose,
base @ pose_0 @ base,
decimal=5)
vertices = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
normals = np.array([[0, 0, 1], [0, 1, 0], [1, 0, 0]])
indices = np.array([0, 1, 2])
mesh = Mesh(vertices, indices, normals)
q1 = Link("", [0.0, 0.0, 1.0], [1.0, 0.0, 0.0],
Link.Type.Revolute,
-3.14,
3.14,
0,
mesh=mesh)
q2 = Link("", [0.0, 0.0, 1.0], [1.0, 0.0, 0.0],
Link.Type.Revolute,
-3.14,
3.14,
0,
mesh=mesh)
s = SerialManipulator("", [q1, q2], base_mesh=mesh)
self.assertEqual(len(s.model(base).meshes), 3)
self.assertEqual(len(s.model(base).transforms), 3)
pose = s.fkine([np.pi / 2, -np.pi / 2])
expected_result = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 0],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(expected_result, pose, decimal=5)
s.set_points = [-np.pi / 2, np.pi / 4]
self.assertAlmostEqual(s.links[0].set_point, -np.pi / 2, 5)
self.assertAlmostEqual(s.links[1].set_point, np.pi / 4, 5)
s.links[0].locked = True
s.links[1].locked = True
s.fkine([-np.pi / 2, np.pi / 2])
expected_result = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 0],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(expected_result,
s.pose,
decimal=5)
pose = s.fkine([-np.pi / 2, np.pi / 2], ignore_locks=True)
expected_result = [[1, 0, 0, 1], [0, 1, 0, -1], [0, 0, 1, 0],
[0, 0, 0, 1]]
np.testing.assert_array_almost_equal(expected_result, pose, decimal=5)
def testSimulationEdgeCases(self):
points = np.rec.array(
[
([0.0, -100.0, 0.0], True),
([0.0, -100.0, 0.0], True),
([0.0, 100.0, 0.0], True),
([0.0, 210.0, 0.0], True),
([-10.0, 10.0, 0.0], True),
([0.0, 100.0, 0.0], True),
],
dtype=POINT_DTYPE,
)
vectors = np.zeros((6, 6, 1), dtype=np.float32)
vectors[:, 0:3, 0] = np.array(self.mock_instrument.q_vectors[0])
vectors[:, 3:6, 0] = np.array(self.mock_instrument.q_vectors[1])
vectors[1, 3:6, 0] = -vectors[1, 3:6, 0]
vectors[2, 3:6, 0] = np.array([-1.0, 0.0, 0.0])
vectors[5, 0:3, 0] = np.array([0.0, 0.0, 0.0])
vectors[5, 3:6, 0] = np.array([0.0, 0.0, 1.0])
simulation = Simulation(self.mock_instrument, self.sample, points,
vectors, self.alignment)
simulation.execute(simulation.args)
simulation.process = self.mock_process
simulation.checkResult()
self.assertEqual(simulation.results[0].ik.status,
IKSolver.Status.Converged)
self.assertEqual(simulation.results[1].ik.status,
IKSolver.Status.Unreachable)
self.assertEqual(simulation.results[2].ik.status,
IKSolver.Status.DeformedVectors)
self.assertEqual(simulation.results[3].ik.status,
IKSolver.Status.HardwareLimit)
self.assertEqual(simulation.results[4].ik.status,
IKSolver.Status.NotConverged)
self.assertEqual(simulation.results[5].ik.status,
IKSolver.Status.Unreachable)
points = np.rec.array([([0.0, -100.0, 0.0], True),
([0.0, -210.0, 0.0], True)],
dtype=POINT_DTYPE)
vectors = np.zeros((2, 6, 1), dtype=np.float32)
vectors[0, 0:3, 0] = np.array(self.mock_instrument.q_vectors[0])
vectors[0, 3:6, 0] = -np.array(self.mock_instrument.q_vectors[1])
self.mock_instrument.positioning_stack.links[0].locked = True
simulation = Simulation(self.mock_instrument, self.sample, points,
vectors, self.alignment)
simulation.execute(simulation.args)
simulation.process = self.mock_process
simulation.checkResult()
self.assertEqual(simulation.results[0].ik.status,
IKSolver.Status.Unreachable)
self.assertEqual(simulation.results[1].ik.status,
IKSolver.Status.HardwareLimit)
q1 = Link("Z", [0.0, 0.0, 1.0], [0.0, 0.0, 0.0], Link.Type.Revolute,
-3.14, 3.14, 0)
q2 = Link("Z2", [0.0, 0.0, -1.0], [0.0, 0.0, 0.0], Link.Type.Revolute,
-3.14, 3.14, 0)
q3 = Link("Y", [0.0, 1.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Prismatic,
-200.0, 200.0, 0)
s = SerialManipulator("", [q1, q2, q3],
custom_order=[2, 1, 0],
base=Matrix44.fromTranslation([0.0, 0.0, 50.0]))
simulation.args["positioner"] = stack_to_string(
PositioningStack(s.name, s))
simulation.results = []
simulation.execute(simulation.args)
simulation.checkResult()
self.assertEqual(simulation.results[0].ik.status,
IKSolver.Status.Unreachable)
q2 = Link("X", [1.0, 0.0, 0.0], [0.0, 0.0, 0.0], Link.Type.Revolute,
-3.14, 3.14, 0)
s = SerialManipulator("", [q1, q2, q3],
custom_order=[2, 1, 0],
base=Matrix44.fromTranslation([0.0, 0.0, 50.0]))
simulation.args["positioner"] = stack_to_string(
PositioningStack(s.name, s))
simulation.results = []
simulation.execute(simulation.args)
simulation.checkResult()
self.assertEqual(simulation.results[0].ik.status,
IKSolver.Status.NotConverged)
def executeButtonClicked(self):
matrix = self.matrix.inverse() if self.invert_checkbox.isChecked(
) else self.matrix
if not is_close(matrix, Matrix44.identity()):
self.parent.presenter.transformSample(matrix, self.selected_sample,
TransformType.Custom)
def setUp(self):
mock_instrument_entity = self.createMock(
"sscanss.core.instrument.simulation.InstrumentEntity")
self.mock_process = self.createMock(
"sscanss.core.instrument.simulation.Process")
self.mock_logging = self.createMock(
"sscanss.core.instrument.simulation.logging")
self.mock_process.is_alive.return_value = False
Collimator = namedtuple("Collimator", ["name"])
Jaws = namedtuple("Jaws", ["beam_direction", "positioner"])
Detector = namedtuple(
"Detector",
["diffracted_beam", "positioner", "current_collimator"])
self.mock_instrument = mock.create_autospec(Instrument)
self.mock_instrument.positioning_stack = self.createPositioningStack()
self.mock_instrument.jaws = Jaws([1.0, 0.0, 0.0],
self.createPositioner())
self.mock_instrument.gauge_volume = [0.0, 0.0, 0.0]
self.mock_instrument.q_vectors = [[-0.70710678, 0.70710678, 0.0],
[-0.70710678, -0.70710678, 0.0]]
self.mock_instrument.detectors = {
"North":
Detector([0.0, 1.0, 0.0], self.createPositioner(),
Collimator("4mm")),
"South":
Detector([0.0, -1.0, 0.0], None, Collimator("2mm")),
}
self.mock_instrument.beam_in_gauge_volume = True
meshes = None
offsets = []
transforms = []
for mesh, transform in self.mock_instrument.positioning_stack.model():
if meshes is None:
meshes = mesh
else:
meshes.append(meshes)
transforms.append(transform)
offsets.append(len(meshes.indices))
beam_stop = create_cuboid(100, 100, 100)
beam_stop.translate([0.0, 100.0, 0.0])
transforms.append(Matrix44.identity())
meshes.append(beam_stop)
offsets.append(len(meshes.indices))
mock_instrument_entity.return_value.vertices = meshes.vertices
mock_instrument_entity.return_value.indices = meshes.indices
mock_instrument_entity.return_value.transforms = transforms
mock_instrument_entity.return_value.offsets = offsets
mock_instrument_entity.return_value.keys = {
"Positioner": 2,
"Beam_stop": 3
}
self.sample = {"sample": create_cuboid(50.0, 100.000, 200.000)}
self.points = np.rec.array(
[([0.0, -90.0, 0.0], True), ([0.0, 0.0, 0.0], True),
([0.0, 90.0, 0.0], True), ([0.0, 0.0, 10.0], False)],
dtype=POINT_DTYPE,
)
self.vectors = np.zeros((4, 6, 1), dtype=np.float32)
self.alignment = Matrix44.identity()