Force and torque implementations

invesalius/constants.py

@@ -752,6 +752,15 @@
 CONSOLE_LOGGING_LEVEL = 9
 LOGGING_LEVEL_TYPES = ["NOTSET", "DEBUG", "INFO", "WARN", "ERROR", "CRITICAL"]
 
+# ------------ Navigation options key ------------
+EXCESSIVE_FORCE_ADJUST = 10
+EXCESSIVE_FORCE_MIN = 11
+EXCESSIVE_FORCE_MAX = 12
+FORCE_SENSOR_CENTERING = 13
+
+# Default value for excessive force adjust
+FORCE_ADJUST_DISTANCE = 4
+
 # Correlaction extracted from pyDicom
 DICOM_ENCODING_TO_PYTHON = {
     "None": "iso8859",

invesalius/data/viewer_volume.py

@@ -256,6 +256,9 @@ def __init__(self, parent):
 
         self.distance_text = None
         self.robot_warnings_text = None
+        self.force_compensate_arrow_up = None
+        # self.force_compensate_text = None
+        self.force_compensate_distance = 0
 
         # self.obj_axes = None
         self.mark_actor = None
@@ -507,6 +510,10 @@ def __bind_events(self):
         Publisher.subscribe(
             self.OnUpdateRobotStatus, "Robot to Neuronavigation: Update robot status"
         )
+        Publisher.subscribe(
+            self.UpdateForceCompensation,
+            "Robot to Neuronavigation: Update force compensation displacement",
+        )
         Publisher.subscribe(self.GetCoilPosition, "Calculate position and rotation")
         Publisher.subscribe(
             self.CreateCortexProjectionOnScalp, "Send efield target position on brain"
@@ -919,6 +926,8 @@ def CreateTargetGuide(self):
         obj_pitch.RotateY(90)
         obj_pitch.RotateZ(180)
 
+        force_compensate_arrow_up = self.actor_factory.CreateArrow([0, 0, 0], [0, 0, 50])
+
         arrow_roll_z1 = self.actor_factory.CreateArrow([-50, -35, 12], [-50, -35, 50])
         arrow_roll_z1.GetProperty().SetColor(1, 1, 0)
         arrow_roll_z1.RotateX(-60)
@@ -957,13 +966,17 @@ def CreateTargetGuide(self):
             arrow_pitch_x1,
             arrow_pitch_x2,
         )
+        print("FORCE_COMP_ARROW_UP_IN_CREATE_TARGET_GUIDE")
+        self.force_compensate_arrow_up = force_compensate_arrow_up
 
         for ind in self.guide_coil_actors:
             self.target_guide_renderer.AddActor(ind)
 
         for ind in self.guide_arrow_actors:
             self.target_guide_renderer.AddActor(ind)
 
+        self.target_guide_renderer.AddActor(force_compensate_arrow_up)
+
     def EnableTargetMode(self):
         # Store the current camera settings so that they can be restored when the target mode is disabled.
         self.stored_camera_settings = self.GetCameraSettings()
@@ -985,10 +998,16 @@ def EnableTargetMode(self):
         if self.distance_text is not None:
             self.ren.RemoveActor(self.distance_text.actor)
 
+        # if self.force_compensate_arrow_up is not None:
+        #     self.ren.RemoveActor(self.force_compensate_arrow_up)
+
         # Create new actor for 'distance' text
         distance_text = self.CreateDistanceText()
         self.ren.AddActor(distance_text.actor)
 
+        # force_compensate_arrow_up = self.CreateCompensateArrowUp()
+        # self.ren.AddActor(force_compensate_arrow_up)
+
         # Store the object for 'distance' text so it can be modified when distance changes.
         self.distance_text = distance_text
 
@@ -1003,6 +1022,16 @@ def EnableTargetMode(self):
         # Store the object for 'distance' text so it can be modified when distance changes.
         self.robot_warnings_text = robot_warnings_text
 
+        ##########################################
+
+        # force_compensate_text = self.CreateForceCompensateText()
+        # self.ren.AddActor(force_compensate_text.actor)
+
+        # # Store the object for 'compensate' text so it can be modified when distance changes.
+        # self.force_compensate_text = force_compensate_text
+
+        ##########################################
+
         self.CreateTargetGuide()
 
         self.ren.ResetCamera()
@@ -1042,6 +1071,11 @@ def DisableTargetMode(self):
         if self.robot_warnings_text is not None:
             self.ren.RemoveActor(self.robot_warnings_text.actor)
 
+        ##########################################
+        # if self.force_compensate_arrow_up is not None:
+        #     self.ren.RemomveActor(self.force_compensate_arrow_up)
+        ##########################################
+
         self.camera_show_object = None
         if self.actor_peel:
             if self.object_orientation_torus_actor:
@@ -1064,18 +1098,37 @@ def SetTargetMode(self, enabled=False):
         else:
             self.DisableTargetMode()
 
+    def UpdateForceCompensation(self, displacement):
+        self.force_compensate_distance = displacement
+        # formatted_force_compensate = "Force Compensate: {: >5.1f} mm".format(displacement)
+        # if self.force_compensate_text is not None:
+        #         self.force_compensate_text.SetValue(formatted_force_compensate)
+
     def OnUpdateCoilPose(self, m_img, coord):
         # vtk_colors = vtkNamedColors()
         if self.target_coord and self.target_mode:
             distance_to_target = distance.euclidean(
                 coord[0:3], (self.target_coord[0], -self.target_coord[1], self.target_coord[2])
             )
 
-            formatted_distance = f"Distance: {distance_to_target: >5.1f} mm"
+            if self.force_compensate_distance < 0:
+                formatted_distance = f"Distance: {distance_to_target: >5.1f} mm UP \u2191"
+                # self.force_compensate_arrow_up.SetOrientation()
+            elif self.force_compensate_distance > 0:
+                formatted_distance = f"Distance: {distance_to_target: >5.1f} mm DOWN ↓\u2193"
+            else:
+                formatted_distance = f"Distance: {distance_to_target: >5.1f} mm"
+
+            # formatted_force_compensate = "Force Compensate: {: >5.1f} mm".format(self.force_compensate_distance)
 
             if self.distance_text is not None:
                 self.distance_text.SetValue(formatted_distance)
 
+            ##########################################
+            # if self.force_compensate_text is not None:
+            #     self.force_compensate_text.SetValue(formatted_force_compensate)
+            ##########################################
+
             self.ren.ResetCamera()
             self.SetCameraTarget()
             if distance_to_target > 100:
@@ -1124,6 +1177,22 @@ def OnUpdateCoilPose(self, m_img, coord):
                 for actor in self.guide_arrow_actors:
                     self.target_guide_renderer.RemoveActor(actor)
 
+            ########
+            if self.force_compensate_arrow_up is not None:
+                self.target_guide_renderer.RemoveActor(self.force_compensate_arrow_up)
+
+            # print("creating force_compensate_arrow_up")
+            # force_compensate_arrow_up = self.actor_factory.CreateArrow([0, 0, 0], [0, 0, 50])
+            force_compensate_arrow_up = self.actor_factory.CreateArrow([0, 0, 0], [0, -50, 0])
+            force_compensate_arrow_up.SetPosition(0, -575, 0)
+            force_compensate_arrow_up.RotateY(90)
+            force_compensate_arrow_up.RotateZ(180)
+            force_compensate_arrow_up.GetProperty().SetColor(1, 1, 1)
+
+            # print("Adding the actor force_compensate_arrow_up")
+            self.target_guide_renderer.AddActor(force_compensate_arrow_up)
+            ########
+
             if (
                 self.angle_threshold * const.ARROW_SCALE
                 > coordrx_arrow
@@ -1290,6 +1359,34 @@ def CreateDistanceText(self):
 
         return distance_text
 
+    # def CreateCompensateArrowUp(self):
+    #     # arrow_source = vtkArrowSource()
+
+    #     force_compensate_arrow_up = self.actor_factory.CreateArrow([0, 0, 0], [10, 10, 10])
+    #     force_compensate_arrow_up.GetProperty().SetColor(1, 1, 0)
+    #     force_compensate_arrow_up.RotateX(-60)
+    #     force_compensate_arrow_up.RotateZ(180)
+    #     return force_compensate_arrow_up
+
+    # def CreateForceCompensateText(self):
+    # up_arrow_actor = vtku.vtkTextActor()
+    # up_arrow_actor.SetInput("TESTTEST")  # Unicode for up arrow
+    # up_arrow_actor.GetTextProperty().SetFontSize(24)
+    # up_arrow_actor.GetTextProperty().SetColor(1.0, 1.0, 1.0)
+    # up_arrow_actor.SetPosition(100, 100) #### Might wanna change
+
+    # return up_arrow_actor
+
+    # def CreateForceCompensateText(self):
+    #     force_compensate_text = vtku.Text()
+
+    #     force_compensate_text.SetSize(20)
+    #     force_compensate_text.SetPosition((0.03, 0.09)) # to do dynamic positioning I'd have to run setPosition in some part of code that runs continuosly
+    #     force_compensate_text.SetVerticalJustificationToBottom()
+    #     force_compensate_text.BoldOn()
+
+    #     return force_compensate_text
+
     def CenterOfMass(self):
         barycenter = [0.0, 0.0, 0.0]
         proj = prj.Project()

invesalius/gui/frame.py

@@ -771,6 +771,9 @@ def ShowPreferences(self, page=0):
             logging_level = values[const.LOGGING_LEVEL]
             append_log_file = values[const.APPEND_LOG_FILE]
             logging_file = values[const.LOGFILE]
+            excessive_force_adjust = values[const.EXCESSIVE_FORCE_ADJUST]
+            excessive_force_distance = values[const.EXCESSIVE_FORCE_MIN]
+            force_centering_on = values[const.FORCE_SENSOR_CENTERING]
 
             session.SetConfig("rendering", rendering)
             session.SetConfig("surface_interpolation", surface_interpolation)
@@ -786,13 +789,17 @@ def ShowPreferences(self, page=0):
             session.SetConfig("logging_level", logging_level)
             session.SetConfig("append_log_file", append_log_file)
             session.SetConfig("logging_file", logging_file)
+            session.SetConfig("excessive_force_adjust", excessive_force_adjust)
+            session.SetConfig("excessive_force_distance", excessive_force_distance)
+            session.SetConfig("force_centering_on", force_centering_on)
 
             Publisher.sendMessage("Remove Volume")
             Publisher.sendMessage("Reset Raycasting")
             Publisher.sendMessage("Update Slice Interpolation")
             Publisher.sendMessage("Update Slice Interpolation MenuBar")
             Publisher.sendMessage("Update Navigation Mode MenuBar")
             Publisher.sendMessage("Update Surface Interpolation")
+            Publisher.sendMessage("Update excessive force adjustment")
 
     def ShowAbout(self):
         """