Adds feature to export positioner pose from a simulation

sscanss/__version.py

@@ -64,5 +64,5 @@ def __repr__(self):
         return f'Version({self.major}, {self.minor}, {self.patch}, {self.pre_release}, {self.build})'
 
 
-__version__ = Version(2, 2, 1)
-__editor_version__ = Version(2, 2, 0)
+__version__ = Version(2, 3, 0, 'dev')
+__editor_version__ = Version(2, 3, 0, 'dev')

sscanss/app/dialogs/misc.py

@@ -719,21 +719,30 @@ def __init__(self, parent):
         button_layout.addWidget(divider)
         button_layout.addSpacing(5)
 
+        self.export_poses_button = create_tool_button(
+            tooltip='Export Pose Matrices',
+            style_name='ToolButton',
+            status_tip='Export pose matrices of the positioner for current simulation',
+            icon='arrow-down.png')
+        self.export_poses_button.clicked.connect(self.parent.presenter.exportPoses)
+
         self.path_length_button = create_tool_button(tooltip='Plot Path Length',
                                                      style_name='ToolButton',
                                                      status_tip='Plot calculated path length for current simulation',
                                                      icon='line-chart.png')
         self.path_length_button.clicked.connect(self.parent.showPathLength)
 
-        self.export_button = create_tool_button(tooltip='Export Script',
-                                                style_name='ToolButton',
-                                                status_tip='Export script for current simulation',
-                                                icon='export.png')
-        self.export_button.clicked.connect(self.parent.showScriptExport)
+        self.export_script_button = create_tool_button(tooltip='Export Script',
+                                                       style_name='ToolButton',
+                                                       status_tip='Export script for current simulation',
+                                                       icon='export.png')
+        self.export_script_button.clicked.connect(self.parent.showScriptExport)
 
         button_layout.addWidget(self.path_length_button)
         button_layout.addSpacing(5)
-        button_layout.addWidget(self.export_button)
+        button_layout.addWidget(self.export_poses_button)
+        button_layout.addSpacing(5)
+        button_layout.addWidget(self.export_script_button)
         main_layout.addLayout(button_layout)
 
         self.progress_label = QtWidgets.QLabel()

sscanss/app/window/presenter.py

@@ -1147,6 +1147,21 @@ def exportScript(self, script_renderer):
                     text_file.write(script_text)
                 return True
             except OSError as e:
-                self.notifyError(f'A error occurred while attempting to save this project ({filename})', e)
+                self.notifyError(f'An error occurred while attempting to export this project script ({filename})', e)
 
         return False
+
+    def exportPoses(self):
+        """Exports simulation poses to text file."""
+        if not self.view.isValidSimulation(self.model.simulation):
+            return
+
+        save_path = f'{os.path.splitext(self.model.save_path)[0]}_poses' if self.model.save_path else ''
+        filename = self.view.showSaveDialog('Text File (*.txt)', current_dir=save_path, title='Export Poses')
+
+        if filename:
+            poses = [result.pose_matrix[:3, :].flatten('F') for result in self.model.simulation.results]
+            try:
+                np.savetxt(filename, poses, delimiter='\t', fmt='%.7f')
+            except OSError as e:
+                self.notifyError(f'An error occurred while attempting to export this simulation poses ({filename})', e)

sscanss/app/window/view.py

@@ -995,11 +995,22 @@ def showCalibrationError(self, pose_id, fiducial_id, error):
         calibration_error = CalibrationErrorDialog(self, pose_id, fiducial_id, error)
         return calibration_error.exec() == QtWidgets.QDialog.DialogCode.Accepted
 
+    def isValidSimulation(self, simulation):
+        if simulation is None:
+            self.showMessage('There are no simulation results.', MessageType.Information)
+            return False
+
+        if simulation.isRunning():
+            self.showMessage('Finish or Stop the current simulation before attempting other actions.',
+                             MessageType.Information)
+            return False
+
+        return True
+
     def showPathLength(self):
         """Opens the path length plotter dialog"""
         simulation = self.presenter.model.simulation
-        if simulation is None:
-            self.showMessage('There are no simulation results.', MessageType.Information)
+        if not self.isValidSimulation(simulation):
             return
 
         if not simulation.compute_path_length:
@@ -1017,13 +1028,7 @@ def showPathLength(self):
     def showScriptExport(self):
         """Shows the dialog for exporting the resulting script from a simulation"""
         simulation = self.presenter.model.simulation
-        if simulation is None:
-            self.showMessage('There are no simulation results to write in script.', MessageType.Information)
-            return
-
-        if simulation.isRunning():
-            self.showMessage('Finish or Stop the current simulation before attempting to write script.',
-                             MessageType.Information)
+        if not self.isValidSimulation(simulation):
             return
 
         if not simulation.has_valid_result:

sscanss/core/instrument/simulation.py

@@ -193,6 +193,8 @@ class SimulationResult:
     :type result_id: str
     :param ik: inverse kinematics result
     :type ik: Optional[IKResult]
+    :param pose_matrix: computed robot pose matrix
+    :type pose_matrix: Optional[Matrix44]
     :param q_formatted: formatted positioner offsets
     :type q_formatted: Tuple
     :param alignment: alignment index
@@ -209,6 +211,7 @@ class SimulationResult:
     def __init__(self,
                  result_id,
                  ik=None,
+                 pose_matrix=Matrix44.identity(),
                  q_formatted=(None, None),
                  alignment=0,
                  path_length=None,
@@ -218,6 +221,7 @@ def __init__(self,
 
         self.id = result_id
         self.ik = ik
+        self.pose_matrix = pose_matrix
         self.alignment = alignment
         self.joint_labels, self.formatted = q_formatted
         self.path_length = path_length
@@ -656,13 +660,16 @@ def execute(args):
                 if exit_event.is_set():
                     break
 
-                result = SimulationResult(label, r, (joint_labels, positioner.toUserFormat(r.q)), j)
+                result = SimulationResult(label,
+                                          r,
+                                          q_formatted=(joint_labels, positioner.toUserFormat(r.q)),
+                                          alignment=j)
                 if r.status != IKSolver.Status.Failed:
-                    pose = positioner.fkine(r.q) @ positioner.tool_link
+                    result.pose_matrix = positioner.fkine(r.q) @ positioner.tool_link
 
                     if compute_path_length and beam_in_gauge:
                         transformed_sample = Node(sample)
-                        matrix = pose.transpose()
+                        matrix = result.pose_matrix.transpose()
                         transformed_sample.vertices = sample.vertices @ matrix[0:3, 0:3] + matrix[3, 0:3]
                         result.path_length = path_length_calculation(transformed_sample, gauge_volume, beam_axis,
                                                                      diff_axis)
@@ -672,7 +679,8 @@ def execute(args):
                         break
 
                     if check_collision:
-                        update_colliders(manager, pose, sample_ids, positioner.model().transforms, positioner_ids)
+                        update_colliders(manager, result.pose_matrix, sample_ids,
+                                         positioner.model().transforms, positioner_ids)
                         result.collision_mask = manager.collide()
 
                 if exit_event.is_set():
@@ -776,7 +784,7 @@ def execute(args):
                     status = IKSolver.Status.HardwareLimit
 
                 r = IKResult(q0, status, [0.] * 3, [0.] * 3, True, True)
-                result = SimulationResult(label, r, (joint_labels, positioner.toUserFormat(q0)))
+                result = SimulationResult(label, r, pose, (joint_labels, positioner.toUserFormat(q0)))
 
                 if exit_event.is_set():
                     break

tests/test_main_presenter.py

@@ -469,6 +469,45 @@ def testExportScript(self, open_func):
         self.assertFalse(self.presenter.exportScript(script_renderer))
         self.notify.assert_called_once()
 
+    @mock.patch("sscanss.app.window.presenter.np.savetxt", autospec=True)
+    def testExportPose(self, savetxt):
+        self.view_mock.isValidSimulation.return_value = False
+        self.model_mock.return_value.simulation = None
+
+        self.presenter.exportPoses()
+        savetxt.assert_not_called()
+
+        self.view_mock.isValidSimulation.return_value = True
+        self.model_mock.return_value.save_path = ""
+        self.view_mock.showSaveDialog.return_value = ""
+        self.presenter.exportPoses()
+        savetxt.assert_not_called()
+
+        simulation = mock.Mock()
+        self.view_mock.showSaveDialog.return_value = "poses.txt"
+        self.model_mock.return_value.simulation = simulation
+
+        result_mock = mock.Mock()
+        result_mock.pose_matrix = np.eye(4)
+        simulation.results = [result_mock]
+        self.presenter.exportPoses()
+        savetxt.assert_called_once()
+        self.assertEqual(savetxt.call_args[0][0], "poses.txt")
+        np.testing.assert_array_equal(savetxt.call_args[0][1], [[1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0]])
+
+        result_mock2 = mock.Mock()
+        result_mock2.pose_matrix = np.array([[1, 2, 3, 4], [5, 6, 7, 8], [9, 10, 11, 12], [0, 0, 0, 1]])
+        simulation.results.append(result_mock2)
+        self.view_mock.showSaveDialog.return_value = "new_poses.txt"
+        self.presenter.exportPoses()
+        self.assertEqual(savetxt.call_args[0][0], "new_poses.txt")
+        np.testing.assert_array_equal(savetxt.call_args[0][1],
+                                      [[1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0], [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]])
+
+        savetxt.side_effect = OSError
+        self.presenter.exportPoses()
+        self.notify.assert_called_once()
+
     @mock.patch("sscanss.app.window.presenter.settings", autospec=True)
     def testSimulationRunAndStop(self, setting_mock):
         self.view_mock.docks = mock.Mock()

tests/test_ui.py

@@ -821,7 +821,7 @@ def runSimulation(self):
         path_length_plotter.close()
         self.assertFalse(path_length_plotter.isVisible())
 
-        QTest.mouseClick(widget.export_button, Qt.MouseButton.LeftButton)
+        QTest.mouseClick(widget.export_script_button, Qt.MouseButton.LeftButton)
         script_exporter = self.window.findChild(ScriptExportDialog)
         self.assertTrue(script_exporter.isVisible())
         script_exporter.close()

tests/test_widgets.py

@@ -168,15 +168,16 @@ def testSimulationResult(self):
         limit = IKResult([87.8], IKSolver.Status.HardwareLimit, (0.0, 0.0, 0.0), (1.0, 1.0, 0.0), True, False)
         unreachable = IKResult([87.8], IKSolver.Status.Unreachable, (0.0, 0.0, 0.0), (1.0, 1.0, 0.0), True, False)
         deformed = IKResult([87.8], IKSolver.Status.DeformedVectors, (0.0, 0.0, 0.0), (1.0, 1.0, 0.0), True, False)
+        pose_matrix = Matrix44([[1, 0, 0, 1], [1, 0, 1, 2], [0, 1, 0, -1], [0, 0, 0, 1]])
 
         self.simulation_mock.results = [
-            SimulationResult("1", converged, (["X"], [90]), 0, (120, ), [False, False]),
-            SimulationResult("2", converged, (["X"], [90]), 0, (120, ), [False, True]),
-            SimulationResult("3", not_converged, (["X"], [87.8]), 0, (25, ), [True, True]),
-            SimulationResult("4", non_fatal, (["X"], [45]), 0),
-            SimulationResult("5", limit, (["X"], [87.8]), 0, (25, ), [True, True]),
-            SimulationResult("6", unreachable, (["X"], [87.8]), 0, (25, ), [True, True]),
-            SimulationResult("7", deformed, (["X"], [87.8]), 0, (25, ), [True, True]),
+            SimulationResult("1", converged, pose_matrix, (["X"], [90]), 0, (120, ), [False, False]),
+            SimulationResult("2", converged, pose_matrix, (["X"], [90]), 0, (120, ), [False, True]),
+            SimulationResult("3", not_converged, pose_matrix, (["X"], [87.8]), 0, (25, ), [True, True]),
+            SimulationResult("4", non_fatal, pose_matrix, (["X"], [45]), 0),
+            SimulationResult("5", limit, pose_matrix, (["X"], [87.8]), 0, (25, ), [True, True]),
+            SimulationResult("6", unreachable, pose_matrix, (["X"], [87.8]), 0, (25, ), [True, True]),
+            SimulationResult("7", deformed, pose_matrix, (["X"], [87.8]), 0, (25, ), [True, True]),
             SimulationResult("8", skipped=True, note="something happened"),
         ]
         self.simulation_mock.count = len(self.simulation_mock.results)
@@ -187,6 +188,9 @@ def testSimulationResult(self):
         self.simulation_mock.result_updated.emit(False)
         self.dialog.filter_button_group.button(3).toggle()
 
+        np.testing.assert_array_equal(self.simulation_mock.results[1].pose_matrix, pose_matrix)
+        np.testing.assert_array_equal(self.simulation_mock.results[7].pose_matrix, Matrix44.identity())
+
         self.assertEqual(self.dialog.result_counts[self.dialog.ResultKey.Good], 1)
         self.assertEqual(self.dialog.result_counts[self.dialog.ResultKey.Warn], 5)
         self.assertEqual(self.dialog.result_counts[self.dialog.ResultKey.Fail], 1)
@@ -1031,9 +1035,9 @@ def setUp(self, model_mock):
         non_fatal = IKResult([45], IKSolver.Status.Failed, (-1.0, -1.0, -1.0), (-1.0, -1.0, -1.0), False, False)
         self.model_mock.return_value.instrument.script = self.template_mock
         self.simulation_mock.results = [
-            SimulationResult("1", converged, (["X"], [90]), 0, (120, ), [False, True]),
-            SimulationResult("3", non_fatal, (["X"], [45]), 0, None, None),
-            SimulationResult("2", not_converged, (["X"], [87.8]), 0, (25, ), [True, True]),
+            SimulationResult("1", converged, Matrix44.identity(), (["X"], [90]), 0, (120, ), [False, True]),
+            SimulationResult("3", non_fatal, Matrix44.identity(), (["X"], [45]), 0, None, None),
+            SimulationResult("2", not_converged, Matrix44.identity(), (["X"], [87.8]), 0, (25, ), [True, True]),
         ]
 
         self.presenter = MainWindowPresenter(self.view)