#Importing Standard Libraries import sys import math import serial import time #Importing PyQt Libraries for GUI from PyQt5.QtWidgets import QApplication, QLabel, QWidget, QHBoxLayout , QVBoxLayout, QGridLayout, QGridLayout, QSizePolicy, QPushButton, QCheckBox, QSlider, QStackedWidget, QScrollArea, QInputDialog from PyQt5.QtGui import QFont, QColor, QVector3D, QQuaternion, QMatrix4x4 from PyQt5.QtCore import Qt, QTimer, QThread, pyqtSignal from PyQt5.Qt3DCore import QEntity, QTransform #Custom Import from gcode_editor import GCodeEditor from PyQt5.Qt3DExtras import ( Qt3DWindow, QPhongAlphaMaterial, QPhongMaterial, QPlaneMesh, QSphereMesh, QCylinderMesh ) from PyQt5.Qt3DRender import ( QPointLight, QPickEvent, QObjectPicker, QPickingSettings ) # ============================================= # SIMULATION CONSTANT PARAMETERS SECTION # ============================================= CAM_DEFAULTS = (181, 20, 700) # CAM_DEFAULT -> -Azimuth -Elevation -Distance # ============================================= # ROBOT ARM LENGTHS (mm) # ============================================= L1 = 159 # Base to elbow length L2 = 155 # Elbow to wrist length L3 = 58 # Wrist to end-effector length # ============================================= # JOINT LIMITS # ============================================= THETA1LIMIT = (-129, 129) THETA2LIMIT = (25, 95) THETA3LIMIT = (53, 180) THETA4LIMIT = (95, 254) # ============================================= # SERIAL COMMUNICATION CONFIGURATION # ============================================= SERIAL_PORT = "COM3" BAUD_RATE = 115200 LINES_PER_BATCH = 16 # Number of lines to send before waiting READY_SIGNAL = "Gcode Buffer Is Empty" # What Arduino sends to indicate it wants more # ============================================= # G-Code GENERATOR CONSTANT PARAMETERS SECTION # ============================================= WRITE_INTERVAL = 0.1 # Time interval (seconds) for G-code generation during linear movements GCODETEMPFILE = "temprobotArm.gcode" # ============================================= # HELPER FUNCTIONS # ============================================= def QuartRotationXYZ(angleX, angleY, angleZ): """ Compute a combined quaternion rotation from XYZ Euler angles (in degrees). Rotation order: X (roll) → Y (pitch) → Z (yaw). Args: angleX (float): Rotation around X-axis (roll) in degrees. angleY (float): Rotation around Y-axis (pitch) in degrees. angleZ (float): Rotation around Z-axis (yaw) in degrees. Returns: QQuaternion: Combined rotation quaternion. """ # Create individual axis rotations qx = QQuaternion.fromAxisAndAngle(QVector3D(1, 0, 0), angleX) qy = QQuaternion.fromAxisAndAngle(QVector3D(0, 1, 0), angleY) qz = QQuaternion.fromAxisAndAngle(QVector3D(0, 0, 1), angleZ) # Combine rotations in Z*Y*X order (applies X first, then Y, then Z) combined_rotation = qz * qy * qx return combined_rotation def IsInRange(value, limit): """ Check if a value lies within a specified range [min, max]. Args: value (float/int): Value to test. limit (tuple/list): (min, max) range bounds (inclusive). Returns: bool: True if value is within limits, False otherwise. Example: >>> IsInRange(5, (1, 10)) True """ return limit[0] <= value <= limit[1] def clamp_angle(angle: float, limits: tuple[float, float]) -> float: """ Clamps the angle between the specified limits. :param angle: float — the input angle :param limits: tuple or list (min_angle, max_angle) :return: float — clamped angle """ min_angle, max_angle = limits return max(min(angle, max_angle), min_angle) # ============================================= # --- Seconday Classes --- # ============================================= class GCodeSenderThread(QThread): progress_signal = pyqtSignal(str) finished_signal = pyqtSignal() progress_value = pyqtSignal(int) error_signal = pyqtSignal(str) def __init__(self, ser, filename, lines_per_batch): super().__init__() self.ser = ser self.filename = filename self.lines_per_batch = lines_per_batch self.running = True def run(self): try: with open(self.filename, 'r') as file: gcode_lines = [l.strip() for l in file if l.strip()] total_lines = len(gcode_lines) self.progress_signal.emit(f"Starting to send {total_lines} lines of G-code...") index = 0 while index < total_lines and self.running: # Send batch for _ in range(self.lines_per_batch): if index >= total_lines or not self.running: break cmd = gcode_lines[index] self.ser.write((cmd + '\n').encode()) self.progress_signal.emit(f"[PC] Sent: {cmd}") index += 1 # Calculate and emit progress progress = int((index / total_lines) * 100) self.progress_value.emit(progress) if not self.running: break # Wait for Arduino self.progress_signal.emit("Waiting for Arduino ready signal...") while self.running: if self.read_serial_for_gcode(): break self.msleep(10) # Non-blocking sleep if self.running: self.progress_signal.emit("✅ All G-code lines sent!") except Exception as e: self.error_signal.emit(f"Error: {e}") finally: self.finished_signal.emit() def read_serial_for_gcode(self): while True: #print("Reading Serial...") line = self.ser.readline().decode(errors='ignore').strip() if not line: break print(f"[Arduino]: {line}") self.progress_signal.emit(f"[Arduino]: {line}") if READY_SIGNAL in line: return True return False def stop(self): self.running = False # ============================================= # MAIN APPLICATION GUI # ============================================= class Robot3DSimulator(QWidget): def __init__(self): # ============================================= # Initialising Class # ============================================= super().__init__() # Calling Class Constructor # ============================================= # CLASS CONSTANTS SECTION # ============================================= self.initialize_sphere_colors() # Saving Sphere colour states to memory self.initialize_fonts() # Initialize application fonts self.initialize_stylesheets() # Adding StyleSheets for use # ============================================= # TRACKERS SECTION # ============================================= self.last_write_time = 0 # Timestamp (in seconds) of last G-code write during continuous write self.SendOnMove = False # SendOnMove State -- Direct Control Mode self.write_on_move = False # WriteOnMove State -- G-Code Generator Mode self.VacState = False # Vac State -- True -> Suction Running -- False -> Suction Off self.gcode_editors = [] # Track open editors # ============================================= # VARIABLE-INITIALIZATION # ============================================= self.ser = None # ============================================= # UI-INITIALIZATION # ============================================= self.initialize_UI() # ============================================= # MODULE-INITIALIZATION # ============================================= self.InitializeSerialTimer() # ============================================= # --- INITIALIZERS --- # -Organises Implementation # -Easily Modify Modules # ============================================= # ============================================= # --- GUI-INITIALIZERS --- # ============================================= def initialize_UI(self): # ============================================= # WINDOW INITIALIZATION # ============================================= self.setWindowTitle("Robot3DSimulator") # Set application window title self.resize(800, 600) # Set initial window dimensions (width, height) # Center the window on screen screen = QApplication.primaryScreen() # Get the primary screen/monitor information screen_geometry = screen.availableGeometry() # Get the available geometry of the screen (excluding taskbars/docks) center_point = screen_geometry.center() # Get the center point coordinates of the screen frame_geom = self.frameGeometry() # Get the frame geometry of our window (including window decorations) frame_geom.moveCenter(center_point) # Move our window's center point to match the screen's center point # Move the window's top-left corner to the calculated position # This effectively centers the window while accounting for its frame size self.move(frame_geom.topLeft()) # ============================================= # UI STYLING # ============================================= self.setStyleSheet(self.stylesheet_main_background_color) # Set main window background color # ============================================= # MAIN LAYOUT CONFIGURATION # ============================================= MainGBox = QGridLayout() # Create main grid layout -- Used to place all components in place in the window # Configure the main application title label Title = QLabel("3D RobotArm Controller") # Create label with display text Title.setSizePolicy(QSizePolicy.Maximum, QSizePolicy.Maximum) # Prevent automatic resizing Title.adjustSize() # Adjust size to fit content Title.setStyleSheet(self.stylesheet_title_label) # Apply pre-defined CSS styling Title.setFont(self.font_title) # Set pre-configured font Title.setAlignment(Qt.AlignCenter) # Center-align text within label # Add title to layout with positioning: # - Row 0, Column 0 # - Spans 1 row and 2 columns # - Horizontally centered MainGBox.addWidget(Title, 0, 0, 1, 2, alignment=Qt.AlignHCenter) # └┬┘└┬┘└┬┘└┬┘ └──┬─────────────────────┘ # │ │ │ │ └─ Optional alignment # │ │ │ └─ Column span (how many columns the widget occupies) # │ │ └─ Row span (how many rows the widget occupies) # │ └─ Starting column (0-indexed) # └─ Starting row (0-indexed) MainGBox.setRowStretch(3, 1) # Configure row stretch factors (row 3 will expand vertically) self.setLayout(MainGBox) # Set MainGBox as the window's central layout # ============================================= # CONTROL PANEL SETUP # ============================================= PanelContainer = QWidget() # Create a container widget to hold control elements # Set size policy to expand in both directions (horizontally and vertically) # QSizePolicy.Policy.Expanding means the widget will use available space PanelContainer.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) # Create a vertical box layout and assign it to the container widget # This will automatically manage the positioning of child widgets self.ControlPanel = QVBoxLayout(PanelContainer) # Apply visual styling to the container (background, borders, etc.) # Uses predefined stylesheet for consistent theming PanelContainer.setStyleSheet(self.stylesheet_side_panel) # Make panel scrollable scroll = QScrollArea() scroll.setWidget(PanelContainer) scroll.setWidgetResizable(True) scroll.setVerticalScrollBarPolicy(Qt.ScrollBarPolicy.ScrollBarAlwaysOn) scroll.setSizePolicy(QSizePolicy.Policy.MinimumExpanding, QSizePolicy.Policy.MinimumExpanding) scroll.setStyleSheet("border: 0px;") # Add the container to the main grid layout: # - Position: Row 1, Column 1 # - Span: 3 rows tall, 1 column wide # - Alignment: Right-justified within its grid cell MainGBox.addWidget(scroll, 1, 1, 3, 1, alignment=Qt.AlignRight) # Configure column stretch factor for column 1 # The '1' weight means this column will share available space proportionally MainGBox.setColumnStretch(1, 1) # ============================================= # COORDINATES DISPLAY # ============================================= self.X = 0 # Initialize X coordinate self.Y = 0 # Initialize Y coordinate self.z = 0 # Initialize Z coordinate self.Orio = 0 # Initialize orientation angle self.ZOrio = 0 # Initialize Z orientation angle # Create coordinate display label with current position values # Format: "X = [value]mm, Y = [value]mm, Z = [value]mm θ: [angle]°" self.CoordinatesPanel = QLabel("X = " + str(self.X) + "mm, Y = " + str(self.Y) + "mm, Z = " + str(self.z) + "mm" + "\n θ: " + str(self.Orio) + "°") self.CoordinatesPanel.setAlignment(Qt.AlignmentFlag.AlignCenter) # Center-align the text within the label # Set fixed height range (100-150px) and width (400px) self.CoordinatesPanel.setMinimumHeight(100) # Minimum vertical size self.CoordinatesPanel.setMaximumHeight(150) # Maximum vertical size self.CoordinatesPanel.setMaximumWidth(400) # Fixed maximum width self.CoordinatesPanel.setMinimumWidth(400) # Fixed minimum width # Configure sizing behavior: # - Horizontal: Expand to fill available space # - Vertical: Fixed height (respects min/max height) self.CoordinatesPanel.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) self.CoordinatesPanel.setFont(self.font_coordinate_panel) # Apply pre-configured font for coordinate display # Set visual styling: # - Dark gray background # - Light gray border with rounded corners # - Internal padding and top margin # - Black text color self.CoordinatesPanel.setStyleSheet(self.stylesheet_coordinate_textbox) self.ControlPanel.addWidget(self.CoordinatesPanel, alignment=Qt.AlignmentFlag.AlignTop) # Add to control panel's vertical layout, aligned to top # ============================================= # COMPONENT INITIALIZATION # ============================================= self.initialize_robot_controller_UI() # Adds sliders for controlling the Robot Arm self.initialize_camera__control() # Adds sliders for controlling Simulation Camera self.initialize_connect_to_serial_button() # Adds Robot Arm Connection Button self.initialize_toggle_switch() self.initialize_action_widget() # ============================================= # 3D VIEW SETUP # ============================================= self.view = Qt3DWindow() self.view.defaultFrameGraph().setClearColor(QColor.fromRgbF(0.1, 0.1, 0.1, 1.0)) # Set 3D view background color # Container for 3D view self.viewWidget = QWidget.createWindowContainer(self.view) self.scene = self.initScene() self.view.setRootEntity(self.scene) # Robot visualization container RobContainer = QWidget() RobLay = QVBoxLayout() RobContainer.setLayout(RobLay) MainGBox.setColumnStretch(0, 3) # Allocate more space for 3D view MainGBox.setColumnStretch(1, 1) # Allocate less space for control panel RobContainer.setStyleSheet(self.stylesheet_robot_view_widget) RobLay.addWidget(self.viewWidget) MainGBox.addWidget(RobContainer, 1, 0, 3, 1) def initialize_robot_controller_UI(self): # ============================================= # ROBOT CONTROLS WIDGET SETUP # ============================================= # Create main container widget for robot controls RobotControlsWidget = QWidget() RobotControlsWidget.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Maximum) # Main vertical layout for all robot controls RobotControls = QVBoxLayout() # ============================================= # SLIDERS CONTAINER SETUP # ============================================= # Create widget and layout for sliders section SlidersWidget = QWidget() Sliders = QVBoxLayout() # Remove margins/padding for compact slider arrangement SlidersWidget.setStyleSheet(""" margin: 0px; padding: 0px; """) SlidersWidget.setLayout(Sliders) # ============================================= # ORIENTATION SLIDER CONFIGURATION # ============================================= # Create and configure main orientation slider self.OrientationSlider = QSlider(Qt.Horizontal) self.OrientationSlider.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) self.OrientationSlider.setFixedHeight(50) # Fixed height for consistent appearance # Custom slider styling: # - Dark gray background with light gray border # - Thin light gray track (groove) # - Teal handle with bright teal border that extends beyond the track # - Compact spacing with bottom margin self.OrientationSlider.setStyleSheet(self.stylesheet_orientation_slider) # Set slider range and connect signal self.OrientationSlider.setMinimum(-90) self.OrientationSlider.setMaximum(90) self.OrientationSlider.valueChanged.connect(self.update_orio) # ============================================= # Z-AXIS SLIDER CONFIGURATION # ============================================= # Create and configure Z-axis slider ZSlider = QSlider(Qt.Horizontal) ZSlider.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) ZSlider.setFixedHeight(50) # Match height of orientation slider ZSlider.setMinimum(THETA1LIMIT[0]) ZSlider.setMaximum(THETA1LIMIT[1]) # Green-themed slider styling ZSlider.setStyleSheet(self.stylesheet_Z_orientation_slider) ZSlider.valueChanged.connect(self.update_Z_orio) # Connecting ZSlider slider to function # ============================================= # LABELS AND UI ELEMENTS # ============================================= # Set main layout for robot controls widget RobotControlsWidget.setLayout(RobotControls) # Create and style section title label RobotControlLabel = QLabel("RobotControls") RobotControlLabel.setFont(self.font_subtitle) RobotControlLabel.setStyleSheet(self.stylesheet_panel_sub_header) # Create and configure orientation display labels self.OrientationLabel = QLabel("End Effector Orientation: " + str(self.Orio) + "°") self.OrientationLabel.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) self.OrientationLabel.setFont(self.font_info_label) self.OrientationLabel.setStyleSheet(self.stylesheet_info_label) self.OrientationLabel.setAlignment(Qt.AlignmentFlag.AlignCenter) self.ZOrioLabel = QLabel("Robot Z Orientation: " + str(self.ZOrio) + "°") self.ZOrioLabel.setFont(self.font_info_label) self.ZOrioLabel.setStyleSheet(self.stylesheet_info_label) self.ZOrioLabel.setAlignment(Qt.AlignmentFlag.AlignCenter) # ============================================= # ASSEMBLE UI COMPONENTS # ============================================= # Add title label to main controls RobotControls.addWidget(RobotControlLabel, alignment=Qt.AlignmentFlag.AlignHCenter) # Add orientation labels and sliders to sliders container Sliders.addWidget(self.OrientationLabel) Sliders.addWidget(self.OrientationSlider) Sliders.addWidget(self.ZOrioLabel) Sliders.addWidget(ZSlider) # Add sliders container to main controls RobotControls.addWidget(SlidersWidget) # Add complete controls to parent panel self.ControlPanel.addWidget(RobotControlsWidget) def initialize_camera__control(self): # ============================================= # CAMERA CONTROLS WIDGET SETUP # ============================================= # Create main container widget for camera controls CameraControlsWidget = QWidget() CameraControlsWidget.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) CameraControlsWidget.setStyleSheet("""""") # Empty stylesheet for future styling # Create main vertical layout for camera controls self.CameraControls = QVBoxLayout() CameraControlsWidget.setLayout(self.CameraControls) # ============================================= # SLIDERS CONTAINER SETUP # ============================================= # Create widget and layout for camera sliders SlidersWidget = QWidget() SlidersWidget.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) Sliders = QVBoxLayout() SlidersWidget.setLayout(Sliders) SlidersWidget.setStyleSheet(""" margin: 0px; padding: 0px; """) # ============================================= # AZIMUTH SLIDER CONFIGURATION # ============================================= # Azimuth (horizontal rotation) control setup self.AzimuthSlider = QSlider(Qt.Horizontal) self.AzimuthSlider.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) self.AzimuthSlider.setFixedHeight(50) # Fixed height for consistent appearance self.AzimuthSlider.setStyleSheet(self.stylesheet_slider) # Initialize azimuth values and labels self.CameraAzimuth = CAM_DEFAULTS[0] self.AzimuthLabel = QLabel("Camera Azimuth: " + str(self.CameraAzimuth) + "°") self.AzimuthLabel.setFont(self.font_info_label) self.AzimuthLabel.setStyleSheet(self.stylesheet_info_label) self.AzimuthLabel.setSizePolicy(QSizePolicy.Policy.Maximum, QSizePolicy.Policy.Maximum) # Add azimuth controls to layout Sliders.addWidget(self.AzimuthLabel, alignment=Qt.AlignmentFlag.AlignHCenter|Qt.AlignmentFlag.AlignTop) Sliders.addWidget(self.AzimuthSlider, alignment=Qt.AlignmentFlag.AlignTop, stretch=1) # Configure azimuth slider range and connections self.AzimuthSlider.setMinimum(0) # Minimum 0° self.AzimuthSlider.setMaximum(360) # Maximum 360° self.AzimuthSlider.setValue(CAM_DEFAULTS[0]) # Set default value self.AzimuthSlider.valueChanged.connect(self.update_azimuth) # Connect to update handler # ============================================= # ELEVATION SLIDER CONFIGURATION # ============================================= # Elevation (vertical angle) control setup self.ElevationSlider = QSlider(Qt.Horizontal) self.ElevationSlider.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) self.ElevationSlider.setFixedHeight(50) # Match other sliders' height self.ElevationSlider.setStyleSheet(self.stylesheet_slider) # Initialize elevation values and labels self.CameraElevation = CAM_DEFAULTS[1] self.ElevationLabel = QLabel("Camera Elevation: " + str(self.CameraElevation) + "°") self.ElevationLabel.setFont(self.font_info_label) self.ElevationLabel.setStyleSheet(self.stylesheet_info_label) self.ElevationLabel.setSizePolicy(QSizePolicy.Policy.Maximum, QSizePolicy.Policy.Maximum) # Add elevation controls to layout Sliders.addWidget(self.ElevationLabel, alignment=Qt.AlignmentFlag.AlignHCenter|Qt.AlignmentFlag.AlignBottom) Sliders.addWidget(self.ElevationSlider, alignment=Qt.AlignmentFlag.AlignTop, stretch=1) # Configure elevation slider range and connections self.ElevationSlider.setMinimum(-90) # Minimum -90° (looking down) self.ElevationSlider.setMaximum(90) # Maximum 90° (looking up) self.ElevationSlider.setValue(CAM_DEFAULTS[1]) # Set default value self.ElevationSlider.valueChanged.connect(self.update_elevation) # Connect to update handler # ============================================= # DISTANCE SLIDER CONFIGURATION # ============================================= # Distance (zoom) control setup self.DistanceSlider = QSlider(Qt.Horizontal) self.DistanceSlider.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) self.DistanceSlider.setFixedHeight(50) # Consistent with other sliders self.DistanceSlider.setStyleSheet(self.stylesheet_slider) # Initialize distance values and labels self.CameraDistance = CAM_DEFAULTS[2] self.DistanceLabel = QLabel("Camera Distance: " + str(self.CameraDistance)) self.DistanceLabel.setFont(self.font_info_label) self.DistanceLabel.setStyleSheet(self.stylesheet_info_label) self.DistanceLabel.setSizePolicy(QSizePolicy.Policy.Maximum, QSizePolicy.Policy.Maximum) # Add distance controls to layout Sliders.addWidget(self.DistanceLabel, alignment=Qt.AlignmentFlag.AlignHCenter|Qt.AlignmentFlag.AlignBottom) # Add elevation label centered horizontally at bottom of its cell # Add elevation slider aligned to top of its cell with vertical stretch enabled # stretch=1 allows it to expand vertically if space is available Sliders.addWidget(self.DistanceSlider, alignment=Qt.AlignmentFlag.AlignTop, stretch=1) # Configure distance slider range and connections self.DistanceSlider.setMinimum(10) # Minimum zoom distance self.DistanceSlider.setMaximum(2000) # Maximum zoom distance self.DistanceSlider.setValue(CAM_DEFAULTS[2]) # Set default value self.DistanceSlider.valueChanged.connect(self.update_distance) # Connect to update handler # ============================================= # FINAL ASSEMBLY # ============================================= # Add camera controls title label CameraControlsLabel = QLabel("Camera Controls") CameraControlsLabel.setStyleSheet(self.stylesheet_panel_sub_header) CameraControlsLabel.setFont(self.font_subtitle) self.CameraControls.addWidget(CameraControlsLabel, alignment=Qt.AlignmentFlag.AlignHCenter) # Add sliders container to main controls self.CameraControls.addWidget(SlidersWidget) # Add complete camera controls to parent panel self.ControlPanel.addWidget(CameraControlsWidget, alignment=Qt.AlignmentFlag.AlignTop) def initialize_connect_to_serial_button(self): # ============================================= # ROBOT CONNECTION BUTTON SETUP # ============================================= # Create and configure the connection button self.CTS = QPushButton("Connect to RobotArm") # Size and layout behavior self.CTS.setSizePolicy(QSizePolicy.Maximum, QSizePolicy.Maximum) # Button won't expand beyond its content self.CTS.setMinimumWidth(120) # Ensure consistent width # Visual styling self.CTS.setFont(self.font_button) # Use predefined button font self.CTS.setStyleSheet(self.stylesheet_connect_to_serial_button) # Add to layout with proper spacing and alignment #self.ControlPanel.addStretch() # Push button to bottom of available space self.ControlPanel.addWidget( self.CTS, alignment=Qt.AlignmentFlag.AlignHCenter # Centered horizontally, bottom-aligned ) # Connect button signal to slot self.CTS.clicked.connect(self.serial_connection_robot) # Trigger connection handler when clicked def initialize_vac_button(self, layout: QVBoxLayout): # ============================================= # VACUUM CONTROL CHECKBOX SETUP # ============================================= # Create checkbox for vacuum control with label VB = QCheckBox(" VAC ") # Spaces for visual padding VB.setSizePolicy(QSizePolicy.Maximum, QSizePolicy.Maximum) # Prevent widget expansion VB.setMinimumWidth(70) # Ensure consistent width VB.setFont(self.font_button) # Use predefined button font # Custom styling for the vacuum control checkbox VB.setStyleSheet(self.stylesheet_vac_button) # Add to control panel with center alignment layout.addWidget(VB, alignment=Qt.AlignmentFlag.AlignHCenter | Qt.AlignmentFlag.AlignTop) # Connect state change to vacuum control function VB.stateChanged.connect(self.update_vac_variable) # Trigger vacuum state update def initialize_send_on_move_checkbox(self, layout: QVBoxLayout): # ============================================= # SERIAL-ON-MOVE CHECKBOX SETUP # ============================================= # Create checkbox for "Send Serial On Move" functionality self.SOM = QCheckBox("Send Serial On Move") # Apply consistent font styling from parent class self.SOM.setFont(self.font_button) # Custom styling for the checkbox (gold/yellow theme) self.SOM.setStyleSheet(self.stylesheet_default_checkbox) # Add to control panel with centered horizontal and bottom alignment layout.addWidget(self.SOM, alignment=Qt.AlignmentFlag.AlignHCenter) # Connect state changes to handler function self.SOM.clicked.connect(self.update_serial_on_move) # Toggle serial-on-move behavior def initialize_send_serial_button(self, layout: QVBoxLayout): # ============================================= # SERIAL COMMAND BUTTON SETUP # ============================================= # Create the serial send button with label STB = QPushButton("Send Serial") # Configure button sizing behavior STB.setSizePolicy(QSizePolicy.Maximum, QSizePolicy.Maximum) # Prevent button from expanding STB.setMinimumWidth(70) # Set fixed minimum width for consistency STB.setFont(self.font_button) # Apply predefined button font # Custom styling for the serial command button STB.setStyleSheet(self.stylesheet_send_serial_button) # Add button to control panel with center alignment layout.addWidget(STB, alignment=Qt.AlignmentFlag.AlignHCenter) #layout.addStretch(1) # Connect button click to serial command handler STB.clicked.connect(self.send_serial_to_robot) # Trigger serial transmission on click def initialize_toggle_switch(self): # ============================================= # MODE TOGGLE UI COMPONENTS # ============================================= # Create and configure mode toggle label self.mode_toggle_label = QLabel("Toggle Mode: Direct Control") self.mode_toggle_label.setFont(self.font_info_label) self.mode_toggle_label.setStyleSheet(self.stylesheet_info_label) self.mode_toggle_label.setAlignment(Qt.AlignmentFlag.AlignCenter) # Center text within label # Add label to control panel with center alignment self.ControlPanel.addWidget(self.mode_toggle_label, alignment=Qt.AlignmentFlag.AlignCenter) # ============================================= # MODE TOGGLE SLIDER SETUP # ============================================= # Create horizontal slider for mode switching (0=Direct Control, 1=G-Code) self.mode_toggle = QSlider(Qt.Horizontal) self.mode_toggle.setRange(0, 1) # Binary mode: 0 or 1 only self.mode_toggle.setFixedWidth(60) # Fixed width for compact toggle appearance self.mode_toggle.setStyleSheet(self.stylesheet_mode_toggle) # Apply custom toggle styling # Add toggle slider to control panel with horizontal center alignment self.ControlPanel.addWidget(self.mode_toggle, alignment=Qt.AlignmentFlag.AlignHCenter) # ============================================= # LAYOUT SPACING # ============================================= # Add flexible space to push subsequent widgets downward self.ControlPanel.addStretch(1) # ============================================= # EVENT HANDLING CONNECTIONS # ============================================= # Connect slider value changes to mode update handler self.mode_toggle.valueChanged.connect(self.update_mode_toggle) # ============================================= # CUSTOM MOUSE HANDLING # ============================================= def mouse_press_event(event): """ Custom mouse press handler for instant toggle behavior. Toggles the slider value on any click within the slider area, providing immediate feedback regardless of click position. Args: event: QMouseEvent containing click information """ # Toggle between 0 and 1 regardless of click position self.mode_toggle.setValue(1 if self.mode_toggle.value() == 0 else 0) event.accept() # Mark event as handled to prevent default slider behavior # Override default mouse press event for instant toggling self.mode_toggle.mousePressEvent = mouse_press_event def initialize_action_widget(self): # ============================================= # ACTION WIDGET SETUP (MODE-SPECIFIC CONTROLS) # ============================================= # Create stacked widget to hold different control sets for each mode self.action_widget = QStackedWidget() # Remove all visual styling to ensure compact appearance self.action_widget.setStyleSheet(""" border: 0px; /* No border */ margin: 0px; /* No external spacing */ padding: 0px; /* No internal padding */ """) # Set size policy: expand horizontally but use minimum vertical space self.action_widget.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Maximum) # ============================================= # MODE WIDGETS CREATION # ============================================= # Create container widgets for each operational mode direct_control_mode_widget = QWidget() # Widget for direct control mode gcode_generator_mode_widget = QWidget() # Widget for G-code generation mode # Remove styling from mode widgets for clean integration direct_control_mode_widget.setStyleSheet(""" border: 0px solid #ffffff; /* Invisible border */ margin: 0px; /* No external spacing */ padding: 0px; /* No internal padding */ """) # Constrain both mode widgets to use minimum vertical space direct_control_mode_widget.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Maximum) gcode_generator_mode_widget.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Maximum) # ============================================= # LAYOUT CONFIGURATION # ============================================= # Create vertical layouts for each mode's control elements direct_control_layout = QVBoxLayout() # Layout for direct control mode gcode_generator_layout = QVBoxLayout() # Layout for G-code generation mode # Assign layouts to their respective mode widgets direct_control_mode_widget.setLayout(direct_control_layout) gcode_generator_mode_widget.setLayout(gcode_generator_layout) # Align both layouts to top to prevent vertical expansion direct_control_layout.setAlignment(Qt.AlignmentFlag.AlignTop) gcode_generator_layout.setAlignment(Qt.AlignmentFlag.AlignTop) # ============================================= # POPULATE MODE CONTROLS # ============================================= # Initialize Direct Control mode interface components self.initialize_vac_button(direct_control_layout) # Vacuum control button self.initialize_send_on_move_checkbox(direct_control_layout) # Serial-on-move toggle self.initialize_send_serial_button(direct_control_layout) # Manual serial send button # Initialize G-Code Generator mode interface components self.initialize_erase_gcode_buttons(gcode_generator_layout) # G-code editing controls self.initialize_vac_write_to_gcode_button(gcode_generator_layout) # Vacuum G-code commands self.initialize_pause_button(gcode_generator_layout) self.initialize_gwrite_button_and_checkbox(gcode_generator_layout) # G-code writing controls # ============================================= # FINAL ASSEMBLY # ============================================= # Add both mode widgets to the stacked widget container self.action_widget.addWidget(direct_control_mode_widget) # Index 0: Direct Control self.action_widget.addWidget(gcode_generator_mode_widget) # Index 1: G-Code Generator # Add the complete action widget to the main control panel # Aligned to top to maintain compact vertical layout self.ControlPanel.addWidget(self.action_widget, alignment=Qt.AlignmentFlag.AlignTop) def initialize_erase_gcode_buttons(self, layout: QVBoxLayout): # ============================================= # G-CODE ERASE BUTTONS SETUP # ============================================= # Create erase buttons for G-code editing operations all_erase = QPushButton("Erase file") # Button to clear entire G-code file line_erase = QPushButton("Erase line") # Button to remove current line only # Create container widget to hold both erase buttons horizontally widget_erase = QWidget() # Remove all margins and padding for compact button grouping widget_erase.setStyleSheet(""" margin: 0px; /* No external spacing */ padding: 0px; /* No internal padding */ """) # Set container to expand horizontally but use minimum vertical space widget_erase.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Maximum) # Create horizontal layout for side-by-side button arrangement layout_erase = QHBoxLayout() # Apply layout to the container widget widget_erase.setLayout(layout_erase) # Add both erase buttons to the horizontal layout layout_erase.addWidget(all_erase) # Left button: Erase entire file layout_erase.addWidget(line_erase) # Right button: Erase current line # Apply consistent font styling to both buttons all_erase.setFont(self.font_button) line_erase.setFont(self.font_button) # Apply red color scheme to indicate destructive operations all_erase.setStyleSheet(self.stylesheet_red_button) # Red styling for file erase line_erase.setStyleSheet(self.stylesheet_red_button) # Red styling for line erase # Remove all layout margins to ensure buttons touch container edges layout_erase.setContentsMargins(0, 0, 0, 0) # Left, Top, Right, Bottom = 0 # Add the complete erase button group to the parent layout layout.addWidget(widget_erase) all_erase.pressed.connect(self.erase_all_lines) line_erase.pressed.connect(self.erase_line) def initialize_vac_write_to_gcode_button(self, layout: QVBoxLayout): # ============================================= # VACUUM G-CODE WRITE BUTTONS SETUP # ============================================= # Create vacuum control buttons for G-code generation on_VTC = QPushButton("Vac On GWrite") # Button to generate vacuum ON G-code command off_VTC = QPushButton("Vac Off GWrite") # Button to generate vacuum OFF G-code command # Create container widget to hold both vacuum control buttons widget_VTC = QWidget() # Create horizontal layout for side-by-side button arrangement layout_VTC = QHBoxLayout() # Apply the horizontal layout to the container widget widget_VTC.setLayout(layout_VTC) # Add both vacuum control buttons to the horizontal layout layout_VTC.addWidget(on_VTC) # Left button: Vacuum ON command layout_VTC.addWidget(off_VTC) # Right button: Vacuum OFF command # Remove all margins from the layout for compact button grouping layout_VTC.setContentsMargins(0, 0, 0, 0) # Left, Top, Right, Bottom = 0 # Apply consistent font styling to both buttons on_VTC.setFont(self.font_button) off_VTC.setFont(self.font_button) # Apply color-coded styling to indicate vacuum state on_VTC.setStyleSheet(self.stylesheet_vac_gwrite_on) # Green/positive styling for ON off_VTC.setStyleSheet(self.stylesheet_vac_gwrite_off) # Red/negative styling for OFF # Optional: Uncomment for visual debugging of container bounds # widget_VTC.setStyleSheet("border: 2px solid #ff0000;") # Add the complete vacuum control button group to the parent layout layout.addWidget(widget_VTC) on_VTC.pressed.connect(lambda: self.write_to_file("M03")) off_VTC.pressed.connect(lambda: self.write_to_file("M05")) def initialize_gwrite_button_and_checkbox(self, layout: QVBoxLayout): """ Initialize G-code write controls with button and auto-write checkbox. Creates a horizontal container with: - GWrite button (left side) - GWrite On Move checkbox (right side) Args: layout: The parent QVBoxLayout to add these controls to """ # ============================================= # G-WRITE BUTTON SETUP # ============================================= # Create the G-code write command button GW = QPushButton("GWrite") # Configure button sizing behavior GW.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Maximum) # Expand horizontally, fixed vertically GW.setMinimumWidth(70) # Set fixed minimum width for visual consistency GW.setFont(self.font_button) # Apply standard button font styling # Apply custom visual styling for the G-write button GW.setStyleSheet(self.stylesheet_send_serial_button) # ============================================= # AUTO-WRITE CHECKBOX SETUP # ============================================= # Create checkbox for automatic G-code writing during movement self.GWOM = QCheckBox("GWrite On Move") # Set size policy to expand horizontally but maintain fixed height self.GWOM.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Maximum) # Apply consistent font styling to match other UI elements self.GWOM.setFont(self.font_button) # Apply custom checkbox styling (gold/yellow theme) self.GWOM.setStyleSheet(self.stylesheet_default_checkbox) # ============================================= # CONTAINER LAYOUT SETUP # ============================================= # Create container widget and horizontal layout for side-by-side arrangement serial_control_widget = QWidget() serial_control_layout = QHBoxLayout() # Remove all layout margins for compact, edge-to-edge component placement serial_control_layout.setContentsMargins(0, 0, 0, 0) # Apply the horizontal layout to the container widget serial_control_widget.setLayout(serial_control_layout) # ============================================= # COMPONENT ASSEMBLY # ============================================= # Add G-write button to left side of horizontal layout serial_control_layout.addWidget(GW) # Add auto-write checkbox to right side with right alignment serial_control_layout.addWidget(self.GWOM, alignment=Qt.AlignmentFlag.AlignRight) # ============================================= # INTEGRATION WITH PARENT LAYOUT # ============================================= # Add the complete control container to the parent vertical layout layout.addWidget(serial_control_widget) # Note: Signal connections are commented out but would be added here: GW.clicked.connect(self.write_pos_to_file) # For manual G-code writing self.GWOM.clicked.connect(self.update_write_on_move) # For auto-write toggle def initialize_pause_button(self, layout: QVBoxLayout): """ Initialize and configure the pause button for G-code generation. Creates a QPushButton for inserting pause commands, configures its appearance and behavior, and adds it to the specified layout. Args: layout (QVBoxLayout): The parent layout to which the button will be added Notes: - Button expands horizontally but has fixed vertical size - Applies consistent styling from class stylesheet - Connects to write_pause_to_file method on click """ # Create button with descriptive text pause_button = QPushButton("Write Pause") # Configure size policy: expand horizontally, fixed vertically pause_button.setSizePolicy( QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Maximum ) # Set minimum width for visual consistency across the UI pause_button.setMinimumWidth(70) # Apply standardized button font styling pause_button.setFont(self.font_button) # Apply predefined stylesheet for consistent appearance pause_button.setStyleSheet(self.stylesheet_write_pause_button) # Add button to the specified layout layout.addWidget(pause_button) # Connect button click to pause writing functionality pause_button.clicked.connect(self.write_pause_to_file) def initialize_fonts(self): """Initialize and configure font styles for the application UI elements. Creates QFont instances with consistent typography settings for: - Window titles - Coordinate displays - Interactive buttons - Subtitles/secondary headings Font Choices: - 'Dosis' for headings (clean, modern sans-serif) - System 'sans-serif' for buttons (maximizes readability) """ # Main application title (large, bold) self.font_title = QFont("Dosis", 30, QFont.Bold) # Coordinate display (medium weight, regular) self.font_coordinate_panel = QFont("Dosis", 14, QFont.Normal) # Interactive buttons (small, system default for clarity) self.font_button = QFont("sans-serif", 8, QFont.Normal) # Interactive buttons (small, system default for clarity) self.font_info_label = QFont("sans-serif", 8, QFont.Normal) # Secondary headings (bold for hierarchy) self.font_subtitle = QFont("Dosis", 15, QFont.Bold) def initialize_stylesheets(self): # Define application-wide stylesheet constants for consistent UI styling self.stylesheet_main_background_color = "background-color: #171717;" # Dark charcoal background self.stylesheet_title_label = """color: #4f4f4f; border: 3px solid #c2c2c2; border-radius: 12px; padding: 8px; background-color: #999999;""" self.stylesheet_side_panel = """background-color: #202020; border-radius: 12px; border-color: #2a2a2a; border-width: 4px; margin-top: 0px; margin-bottom: 2px; border-style: solid; padding: 15px 15px 0px 0px;""" self.stylesheet_coordinate_textbox = """background-color: #414141; border: 3px solid #9A9A9A; padding: 10px; margin-top: 0px; border-radius: 8px; color: #000000;""" self.stylesheet_orientation_slider = """ QSlider{ margin-bottom: 4px; background-color: #414141; border: 3px solid #9A9A9A; padding: 0px; margin-top: 0px; margin-bottom: 8px; } QSlider::groove:horizontal { background-color: #bcbcbc ; height: 3px; } QSlider::handle:horizontal { background-color: #179999; border: 3px solid #1ec8c8; width: 5px; border-radius: 5px; margin: -15px 0px; } """ self.stylesheet_Z_orientation_slider = """ QSlider { margin-bottom: 4px; background-color: #488536; border: 3px solid #33be2c; padding: 0px; margin-top: 0px; margin-bottom: 8px; } QSlider::groove:horizontal { background-color: #23471d; height: 3px; } QSlider::handle:horizontal { background-color: #116f11; border: 3px solid #1ec81e; width: 5px; border-radius: 5px; margin: -15px 0px; } """ self.stylesheet_panel_sub_header = """ background-color: #414141; border: 3px solid #9A9A9A; color: #d0bd33; padding: 10px; """ self.stylesheet_info_label = """ color: #909090; border: 0px; margin: 5px; padding: 0px; """ self.stylesheet_slider = """ QSlider { margin-bottom: 4px; } QSlider::groove:horizontal { background-color: #e9e9e9; height: 3px; } QSlider::handle:horizontal { background-color: #179999; border: 3px solid #1ec8c8; width: 5px; border-radius: 5px; margin: -15px 0px; } """ self.stylesheet_connect_to_serial_button = """ QPushButton { background-color: #484848; /* Dark gray background */ border: 4px solid #6c6c6c; /* Medium gray border */ padding: 4px; /* Internal spacing */ border-radius: 8px; /* Rounded corners */ margin: 2px; /* External spacing */ margin-bottom: 0px; } QPushButton:pressed { background-color: #3e3e3e; /* Slightly darker when pressed */ } """ self.stylesheet_vac_button = """ QCheckBox { Background-Color: #06b0b0; /* Teal background color */ Border-radius: 4px; /* Slightly rounded corners */ Border: 3px solid #0ff7f7; /* Bright teal border */ padding-left: 8px; /* Horizontal padding */ padding-right: 8px; padding-top: 4px; /* Vertical padding */ padding-bottom: 4px; margin: 2px; /* External spacing */ margin-top: 2px; } """ self.stylesheet_vac_gwrite_on = """ QPushButton { Background-Color: #06b0b0; /* Teal background color */ Border-radius: 4px; /* Slightly rounded corners */ Border: 3px solid #0ff7f7; /* Bright teal border */ padding-left: 8px; /* Horizontal padding */ padding-right: 8px; padding-top: 4px; /* Vertical padding */ padding-bottom: 4px; margin: 2px; /* External spacing */ margin-bottom: 2px; } QPushButton:pressed { Background-Color: #047b7b; } """ self.stylesheet_vac_gwrite_off = """ QPushButton { Background-Color: #c03a01; Border-radius: 4px; /* Slightly rounded corners */ Border: 3px solid #e04401; /* Bright teal border */ padding-left: 8px; /* Horizontal padding */ padding-right: 8px; padding-top: 4px; /* Vertical padding */ padding-bottom: 4px; margin: 2px; /* External spacing */ margin-bottom: 0px; margin-top: 0px; } QPushButton::Pressed { Background-Color: #8f2c01; } """ self.stylesheet_send_serial_button = """ /* Base button style */ QPushButton { Background-Color: rgb(189, 132, 27); /* Orange-brown background */ Border-radius: 4px; /* Slightly rounded corners */ Border: 3px solid rgb(255, 176, 66); /* Brighter orange border */ padding-left: 8px; /* Horizontal padding */ padding-right: 8px; padding-top: 4px; /* Vertical padding */ padding-bottom: 4px; margin: 2px; /* External spacing */ } /* Pressed state style */ QPushButton:pressed { background-color: rgb(158, 106, 24); /* Darker shade when pressed */ } """ self.stylesheet_default_checkbox = """ QCheckBox { Background-Color: #c1ac17; /* Gold background */ Border-radius: 4px; /* Slightly rounded corners */ Border: 3px solid #e2ca1b; /* Brighter gold border */ padding-left: 8px; /* Horizontal padding */ padding-right: 8px; padding-top: 4px; /* Vertical padding */ padding-bottom: 4px; margin-top: 2px; /* Vertical spacing */ margin-bottom: 2px; } """ self.stylesheet_red_button = """ QPushButton{ background-color: #910000; border: 4px solid #d90000; padding: 4px; border-radius: 8px; margin:2px; } QPushButton:Pressed{ background-color: #7b0000; } """ self.stylesheet_write_pause_button = """ QPushButton{ background-color: #7400c1; border: 4px solid #a41cff; padding: 4px; border-radius: 8px; margin:2px; } QPushButton:Pressed{ background-color: #570091; } """ self.stylesheet_mode_toggle = """ QSlider{ border-color: #b4b4b4; border-width: 2px; padding:0px; margin: 0px; margin: 0px; } QSlider::handle:horizontal{ background-color: #cfcfcf; border: 3px solid #9b9b9b; height: 20px; width: 20px; margin: -3px 0px; border-radius: 10px; } QSlider::groove:horizontal { background-color: #bcbcbc ; height: 20px; border-radius: 10px; } QSlider::add-page:horizontal { background-color: #f0871e; border-radius: 10px; margin-right: 1.5px; } QSlider::sub-page:horizontal { background-color: #50eb3f; border-radius: 10px; margin-left: 1.5px; } """ self.stylesheet_robot_view_widget = """border-radius: 12px; border: 6px solid #8d8d8d;""" # ============================================= # --- 3D-SCENE-INITIALIZERS --- # ============================================= def initScene(self): """ Initialize and configure the 3D scene for robotic arm visualization. Creates the root entity, sets up camera, lighting, coordinate system, and all visual components including the robotic arm and reference grid. """ # ============================================= # SCENE ROOT AND INITIALIZATION # ============================================= # Create root entity for the entire 3D scene rootEntity = QEntity() self.dragging = False # Flag for drag interaction state # Configure picking settings for object selection self.view.renderSettings().pickingSettings().setPickMethod(QPickingSettings.TrianglePicking) # ============================================= # CAMERA CONFIGURATION # ============================================= # Convert spherical coordinates to radians for camera positioning azimuth_rad = math.radians(self.CameraAzimuth) elevation_rad = math.radians(self.CameraElevation) # Calculate 3D camera position from spherical coordinates x = self.CameraDistance * math.cos(elevation_rad) * math.sin(azimuth_rad) y = self.CameraDistance * math.cos(elevation_rad) * math.cos(azimuth_rad) z = self.CameraDistance * math.sin(elevation_rad) # Get and configure the camera camera = self.view.camera() camera.lens().setPerspectiveProjection(45.0, 16/9, 0.1, 1000) # Field of view, aspect ratio, near/far planes camera.setNearPlane(1.0) # Minimum visible distance camera.setFarPlane(10000.0) # Maximum visible distance # Set camera position and orientation camera.setPosition(QVector3D(x, y, z)) # Position in 3D space camera.setViewCenter(QVector3D(0, 0, 0)) # Look at origin camera.setUpVector(QVector3D(0, 0, 1)) # Z-axis is up # ============================================= # LIGHTING SYSTEM SETUP # ============================================= # Primary light source (right side) lightEntity = QEntity(rootEntity) light = QPointLight(lightEntity) SunSphereMaterial = QPhongMaterial() SunSphereMaterial.setAmbient(QColor.fromRgbF(1, 1, 1)) # White ambient light SunSphere = QSphereMesh() SunSphere.setRadius(10) # Small sphere to represent the light source visually light.setColor(QColor.fromRgbF(1, 1, 1)) # White light color light.setIntensity(1.5) # Light intensity multiplier # Assemble primary light components lightEntity.addComponent(light) lightEntity.addComponent(SunSphere) lightEntity.addComponent(SunSphereMaterial) lightTransform = QTransform() lightTransform.setTranslation(QVector3D(500, 0, 300)) # Position light above and to the right lightEntity.addComponent(lightTransform) # Secondary light source (left side for balanced lighting) lightEntity2 = QEntity(rootEntity) light2 = QPointLight(lightEntity) SunSphereMaterial2 = QPhongMaterial() SunSphereMaterial2.setAmbient(QColor.fromRgbF(1, 1, 1)) # White ambient light SunSphere2 = QSphereMesh() SunSphere2.setRadius(10) # Visual representation sphere # Configure secondary light light2.setColor(QColor.fromRgbF(1, 1, 1)) # White light color light2.setIntensity(1.5) # Light intensity multiplier # Assemble secondary light components lightEntity2.addComponent(light2) lightEntity2.addComponent(SunSphere2) lightEntity2.addComponent(SunSphereMaterial2) lightTransform2 = QTransform() lightTransform2.setTranslation(QVector3D(-500, 0, 300)) # Position light above and to the left lightEntity2.addComponent(lightTransform2) # ============================================= # SCENE COMPONENTS CREATION # ============================================= # Create reference grid for spatial orientation self.create_grid(rootEntity) # Initialize robotic arm joint angles self.Joint1Angle = THETA2LIMIT[1] # Base joint angle self.Joint2Angle = THETA3LIMIT[0] # Elbow joint angle self.Joint3Angle = THETA4LIMIT[0] # Wrist joint angle # Create robotic arm visual representation self.create_robot_arm(rootEntity) # Create inverse kinematics visualization grid self.create_inverse_kinematic_grid(rootEntity) # ============================================= # COORDINATE SYSTEM VISUALIZATION # ============================================= # Create colored axes for 3D orientation reference self.create_axis(rootEntity, QVector3D(1, 0, 0), color=QColor("red")) # X-axis (red) self.create_axis(rootEntity, QVector3D(0, 1, 0), color=QColor("green")) # Y-axis (green) self.create_axis(rootEntity, QVector3D(0, 0, 1), color=QColor("blue")) # Z-axis (blue) # Return the fully configured root entity return rootEntity def create_grid(self, parentEntity, size=1000, step=50, color=QColor.fromRgbF(0.5, 0.5, 0.5)): """ Create a 3D reference grid for spatial orientation in the scene. Generates a grid of lines in the X-Y plane at Z=0 to provide visual reference for positioning and scale in the 3D environment. Args: parentEntity: The parent QEntity to attach grid components to size: Total grid dimensions (centered at origin, default=1000 units) step: Spacing between grid lines (default=50 units) color: Grid line color (default=mid-gray) """ # ============================================= # GRID MATERIAL SETUP # ============================================= # Create and configure material for all grid lines material = QPhongMaterial(parentEntity) material.setAmbient(color) # Base color under any lighting material.setDiffuse(color) # Main surface color material.setSpecular(QColor(0, 0, 0)) # Remove specular highlights to avoid shine # Grid line visual properties line_radius = 0.2 # Thickness/radius of the grid lines # ============================================= # VERTICAL GRID LINES (ALONG Y-AXIS) # ============================================= # Create lines parallel to Y-axis (vertical in X-Y plane) for x in range(-size, size + 1, step): # Create entity for this grid line lineEntity = QEntity(parentEntity) # Create cylinder mesh for the line line = QCylinderMesh() line.setRadius(line_radius) # Set line thickness line.setLength(size * 2) # Full grid length line.setRings(4) # Radial segmentation for smoothness line.setSlices(16) # Longitudinal segmentation # Add components to line entity lineEntity.addComponent(line) lineEntity.addComponent(material) # Position and orient the line transform = QTransform() # transform.setRotationZ(90) # Alternative rotation method (commented) transform.setTranslation(QVector3D(x, 0, 0)) # Position along X-axis lineEntity.addComponent(transform) # ============================================= # HORIZONTAL GRID LINES (ALONG X-AXIS) # ============================================= # Create lines parallel to X-axis (horizontal in X-Y plane) for y in range(-size, size + 1, step): # Create entity for this grid line lineEntity = QEntity(parentEntity) # Create cylinder mesh for the line line = QCylinderMesh() line.setRadius(line_radius) # Consistent thickness with vertical lines line.setLength(size * 2) # Full grid length line.setRings(4) # Radial segmentation line.setSlices(16) # Longitudinal segmentation # Add components to line entity lineEntity.addComponent(line) lineEntity.addComponent(material) # Position and orient the line (rotate 90° around Z-axis) transform = QTransform() transform.setRotation(QuartRotationXYZ(0, 0, 90)) # Rotate from Z to X axis orientation transform.setTranslation(QVector3D(0, y, 0)) # Position along Y-axis lineEntity.addComponent(transform) def create_axis(self, parent, direction, length=10000.0, radius=0.5, color=QColor("red")): """ Create a 3D axis representation as a cylindrical entity. Args: parent (QEntity): The parent 3D entity to attach the axis to. direction (QVector3D): The direction of the axis (e.g., X, Y, or Z). length (float, optional): Length of the axis cylinder. Default is 10000.0. radius (float, optional): Radius of the axis cylinder. Default is 0.5. color (QColor, optional): Color of the axis. Default is red. Returns: QEntity: The constructed 3D axis entity. """ # =============================== # Create the axis entity # =============================== axis = QEntity(parent) # =============================== # Define cylinder mesh (geometry) # =============================== mesh = QCylinderMesh() mesh.setLength(length) # Set cylinder length mesh.setRadius(radius) # Set cylinder radius (thickness) mesh.setRings(10) # Number of horizontal rings for smoothness mesh.setSlices(20) # Number of vertical slices for roundness # =============================== # Configure material (color/appearance) # =============================== material = QPhongMaterial() material.setDiffuse(color) # Set axis color # =============================== # Configure transformation # =============================== transform = QTransform() # Translate axis so it starts at origin and extends along its direction transform.setTranslation(direction.normalized() * (length / 2)) # =============================== # Adjust cylinder orientation based on axis direction # =============================== if direction == QVector3D(1, 0, 0): # X-axis transform.setRotationX(0) transform.setRotationZ(90) elif direction == QVector3D(0, 1, 0): # Y-axis transform.setRotationX(0) elif direction == QVector3D(0, 0, 1): # Z-axis transform.setRotationX(90) # =============================== # Assemble components # =============================== axis.addComponent(mesh) # Geometry axis.addComponent(material) # Appearance axis.addComponent(transform) # Position & orientation return axis def create_inverse_kinematic_grid(self, parentEntity, size=350, step=10, color=QColor(235, 231, 0)): """ Create an inverse kinematic working plane grid for visualization. This generates: - A transparent plane mesh. - Vertical and horizontal grid lines. - Mouse interaction via a picker (click, drag, move). Args: parentEntity (QEntity): The parent 3D entity to attach the grid to. size (int, optional): Half-size of the grid along X and Y axes. Default is 350. step (int, optional): Distance between grid lines. Default is 10. color (QColor, optional): Grid color. Default is bright yellow. Returns: None """ # =============================== # Setup grid material (semi-transparent) # =============================== self.planeGridMaterial = QPhongAlphaMaterial(parentEntity) self.planeGridMaterial.setAmbient(color) # Base color under lighting self.planeGridMaterial.setDiffuse(color) # Surface color self.planeGridMaterial.setSpecular(QColor(0, 0, 0)) # Disable shiny highlights # =============================== # Initialize main grid entity # =============================== line_radius = 0.2 # Thickness of grid lines self.GridEntity = QEntity(parentEntity) # Root container for grid self.GridTransform = QTransform() self.GridTransform.setRotation(QuartRotationXYZ(0, 90, -90)) # Align grid plane with world axes # =============================== # Plane mesh (interactive picking surface) # =============================== GridMeshEntity = QEntity(self.GridEntity) GridMesh = QPlaneMesh() GridMesh.setWidth(size * 2) # Plane spans full X range GridMesh.setHeight(size * 2) # Plane spans full Y range GridMeshEntity.addComponent(GridMesh) # =============================== # Add interactive picker for mouse events # =============================== picker = QObjectPicker() picker.pressed.connect(self.on_plane_click) # Handle clicks picker.moved.connect(self.on_mouse_move) # Handle mouse movement picker.released.connect(self.on_mouse_release) # Handle release picker.setEnabled(True) picker.setHoverEnabled(False) # Ignore hover if only clicks are needed picker.setDragEnabled(True) # Allow drag interactions GridMeshEntity.addComponent(picker) # =============================== # Plane transform (orient plane correctly) # =============================== GridMeshTransform = QTransform() GridMeshTransform.setRotation(QuartRotationXYZ(90, 0, 0)) GridMeshEntity.addComponent(GridMeshTransform) # =============================== # Draw vertical grid lines (along Y-axis) # =============================== for x in range(-size, size + 1, step): lineEntity = QEntity(self.GridEntity) # Define cylinder mesh for the line line = QCylinderMesh() line.setRadius(line_radius) line.setLength(size * 2) # Spans across full Y range line.setRings(4) line.setSlices(16) # Attach components to entity lineEntity.addComponent(line) lineEntity.addComponent(self.planeGridMaterial) # Position the line along the X-axis transform = QTransform() transform.setTranslation(QVector3D(x, 0, 0)) lineEntity.addComponent(transform) # =============================== # Draw horizontal grid lines (along X-axis) # =============================== for y in range(-size, size + 1, step): lineEntity = QEntity(self.GridEntity) # Define cylinder mesh for the line line = QCylinderMesh() line.setRadius(line_radius) line.setLength(size * 2) # Spans across full X range line.setRings(4) line.setSlices(16) # Attach components to entity lineEntity.addComponent(line) lineEntity.addComponent(self.planeGridMaterial) # Rotate and position the line along the Y-axis transform = QTransform() transform.setRotation(QuartRotationXYZ(0, 0, 90)) # Rotate from Z to X-axis transform.setTranslation(QVector3D(0, y, 0)) lineEntity.addComponent(transform) # =============================== # Attach root grid transform to root entity # =============================== self.GridEntity.addComponent(self.GridTransform) def create_robot_arm(self, parentEntity): """ Create a 3D robotic arm composed of joints (spheres) and segments (cylinders). The robotic arm consists of: - 4 Joints (spherical meshes) - 3 Segments (cylindrical meshes) Each joint and segment has its own transform for positioning and rotation. Args: parentEntity (QEntity): The parent 3D entity to attach the robotic arm to. Returns: None """ # =============================== # Define joint and segment materials # =============================== Joint1Material = QPhongMaterial(parentEntity) Joint1Material.setDiffuse(QColor("red")) # Joint 1: Red Joint2Material = QPhongMaterial(parentEntity) Joint2Material.setDiffuse(QColor("green")) # Joint 2: Green Joint3Material = QPhongMaterial(parentEntity) Joint3Material.setDiffuse(QColor("blue")) # Joint 3: Blue Joint4Material = QPhongMaterial(parentEntity) Joint4Material.setDiffuse(QColor("orange")) # Joint 4: Orange SegmentMaterial = QPhongMaterial(parentEntity) SegmentMaterial.setDiffuse(QColor("gray")) # Segments: Gray # ======================================================= # ------------------ JOINT 1 SETUP ---------------------- # ======================================================= # Mesh and transform for Joint 1 (base joint) Joint1Mesh = QSphereMesh() Joint1Mesh.setRadius(10) # Base joint sphere radius self.Joint1Transform = QTransform() Rot1 = QuartRotationXYZ(0, 0, 180) # Rotate sphere to align with Z-axis Rot2 = QuartRotationXYZ(180, 0, 0) * Rot1 self.PosRot = Rot2 # Set initial rotation based on current angle + orientation offset self.Joint1Transform.setRotation( QuartRotationXYZ(self.Joint1Angle, 0, -90) * self.PosRot ) # Create entity for Joint 1 and attach components self.Joint1Entity = QEntity(parentEntity) self.Joint1Entity.setObjectName("Joint1Entity") self.Joint1Entity.addComponent(Joint1Material) self.Joint1Entity.addComponent(Joint1Mesh) self.Joint1Entity.addComponent(self.Joint1Transform) # ======================================================= # ---------------- SEGMENT 1 (Joint1 → Joint2) ---------- # ======================================================= self.Segment1Mesh = QCylinderMesh() self.Segment1Mesh.setRadius(2) # Cylinder thickness self.Segment1Mesh.setLength(L1) # Length between Joint1 and Joint2 self.Segment1Transform = QTransform() self.Segment1Transform.setTranslation(QVector3D(0, L1 / 2, 0)) # Position midway self.Segment1Entity = QEntity(self.Joint1Entity) self.Segment1Entity.setObjectName("Segment1Entity") self.Segment1Entity.addComponent(SegmentMaterial) self.Segment1Entity.addComponent(self.Segment1Mesh) self.Segment1Entity.addComponent(self.Segment1Transform) # ======================================================= # ------------------ JOINT 2 SETUP ---------------------- # ======================================================= self.Joint2Mesh = QSphereMesh() self.Joint2Mesh.setRadius(10) self.Joint2Transform = QTransform() self.Joint2Transform.setTranslation(QVector3D(0, L1 / 2, 0)) self.Joint2Transform.setRotation( QuartRotationXYZ(-self.Joint2Angle + 180, 0, 0) ) self.Joint2Entity = QEntity(self.Segment1Entity) self.Joint2Entity.setObjectName("Joint2Entity") self.Joint2Entity.addComponent(Joint2Material) self.Joint2Entity.addComponent(self.Joint2Mesh) self.Joint2Entity.addComponent(self.Joint2Transform) # ======================================================= # ---------------- SEGMENT 2 (Joint2 → Joint3) ---------- # ======================================================= self.Segment2Mesh = QCylinderMesh() self.Segment2Mesh.setRadius(2) self.Segment2Mesh.setLength(L2) self.Segment2Transform = QTransform() self.Segment2Transform.setTranslation(QVector3D(0, L2 / 2, 0)) self.Segment2Entity = QEntity(self.Joint2Entity) self.Segment2Entity.setObjectName("Segment2Entity") self.Segment2Entity.addComponent(SegmentMaterial) self.Segment2Entity.addComponent(self.Segment2Mesh) self.Segment2Entity.addComponent(self.Segment2Transform) # ======================================================= # ------------------ JOINT 3 SETUP ---------------------- # ======================================================= self.Joint3Mesh = QSphereMesh() self.Joint3Mesh.setRadius(10) self.Joint3Transform = QTransform() self.Joint3Transform.setTranslation(QVector3D(0, L2 / 2, 0)) self.Joint3Transform.setRotation( QuartRotationXYZ(-self.Joint3Angle + 180, 0, 0) ) self.Joint3Entity = QEntity(self.Segment2Entity) self.Joint3Entity.setObjectName("Joint3Entity") self.Joint3Entity.addComponent(Joint3Material) self.Joint3Entity.addComponent(self.Joint3Mesh) self.Joint3Entity.addComponent(self.Joint3Transform) # ======================================================= # ---------------- SEGMENT 3 (Joint3 → Joint4) ---------- # ======================================================= self.Segment3Mesh = QCylinderMesh() self.Segment3Mesh.setRadius(2) self.Segment3Mesh.setLength(L3) self.Segment3Transform = QTransform() self.Segment3Transform.setTranslation(QVector3D(0, L3 / 2, 0)) self.Segment3Entity = QEntity(self.Joint3Entity) self.Segment3Entity.setObjectName("Segment3Entity") self.Segment3Entity.addComponent(SegmentMaterial) self.Segment3Entity.addComponent(self.Segment3Mesh) self.Segment3Entity.addComponent(self.Segment3Transform) # ======================================================= # ------------------ JOINT 4 SETUP ---------------------- # ======================================================= self.Joint4Mesh = QSphereMesh() self.Joint4Mesh.setRadius(10) self.Joint4Transform = QTransform() self.Joint4Transform.setTranslation(QVector3D(0, L3 / 2, 0)) self.Joint4Entity = QEntity(self.Segment3Entity) self.Joint4Entity.setObjectName("Joint4Entity") self.Joint4Entity.addComponent(Joint4Material) self.Joint4Entity.addComponent(self.Joint4Mesh) self.Joint4Entity.addComponent(self.Joint4Transform) def initialize_sphere_colors(self): """ Initialize the color scheme for sphere materials and their glow effects. Defines RGBA color values for both normal and error states of: - The sphere material itself - The glow effect surrounding the sphere Color Scheme: - Normal State: - Sphere: Pale yellow (245, 244, 169, 150) - Glow: Bright yellow (231, 235, 0, 200) - Error State: - Sphere: Solid red (255, 0, 0, 150) - Glow: Red (255, 0, 0, 150) Note: - Alpha values (transparency): - 150 for semi-transparent effects - 200 for more opaque effects """ # Sphere appearance colors (RGBA format) self.sphere_material_color_norm = QColor(245, 244, 169, 150) # Normal state: Pale yellow (semi-transparent) self.sphere_material_color_bad = QColor(255, 0, 0, 150) # Error state: Solid red (semi-transparent) # Glow effect colors (RGBA format) self.glow_material_colorNorm = QColor(231, 235, 0, 200) # Normal glow: Bright yellow (more opaque) self.glow_material_colorBad = QColor(255, 0, 0, 150) # Error glow: Red (semi-transparent) # ============================================= # --- 3D-SCENE-OPERATORS --- # -Functions that actively alter the 3D-Scene # but are not considered "updaters" # ============================================= def spawn_sphere_at(self, pos3d): """ Create and position a visual sphere entity with glow effect at the specified 3D position. If a sphere already exists, it is removed before creating a new one. Args: pos3d (QVector3D): 3D position where the sphere should be placed """ # Remove existing sphere entity if it exists if hasattr(self, 'sphere_entity') and self.sphere_entity is not None: self.sphere_entity.setParent(None) # Detach from scene self.sphere_entity = None # Create new sphere entity self.sphere_entity = QEntity(self.scene) # Configure main sphere mesh mesh = QSphereMesh() mesh.setRadius(10) # Set sphere size # Configure main sphere material (semi-transparent yellow) self.Spherematerial = QPhongAlphaMaterial() self.Spherematerial.setDiffuse(QColor(245, 244, 169, 150)) # Semi-transparent yellow self.Spherematerial.setAmbient(QColor(245, 244, 169, 150)) # Ambient color # Create glow effect as a child entity of the main sphere glow_entity = QEntity(self.sphere_entity) # Configure glow mesh (slightly larger than main sphere) glow_mesh = QSphereMesh() glow_mesh.setRadius(12) # Configure glow material self.glow_material = QPhongAlphaMaterial() self.glow_material.setAmbient(QColor(231, 235, 0, 200)) # Yellow glow with transparency # Add components to glow entity glow_entity.addComponent(glow_mesh) glow_entity.addComponent(self.glow_material) # Configure transform to position the sphere self.SphereTransform = QTransform() self.SphereTransform.setTranslation(pos3d) # Add all components to the main sphere entity self.sphere_entity.addComponent(mesh) self.sphere_entity.addComponent(self.Spherematerial) self.sphere_entity.addComponent(self.SphereTransform) # ============================================= # --- GUI-Updaters --- # -Deal with Event triggers from GUI # ============================================= def update_azimuth(self, CameraAzimuth): """ Update the camera's azimuth angle and refresh the camera view. This function sets the new azimuth value, updates the corresponding UI label, and triggers a camera update to apply the change. Args: CameraAzimuth (float): New azimuth angle in degrees """ # Update the azimuth angle self.CameraAzimuth = CameraAzimuth # Update the UI label to display the current azimuth value self.AzimuthLabel.setText("Camera Azimuth: " + str(self.CameraAzimuth) + "°") # Refresh the camera position and orientation self.update_camera() def update_elevation(self, CameraElevation): """ Update the camera's elevation angle and refresh the camera view. This function sets the new elevation value, updates the corresponding UI label, and triggers a camera update to apply the change. Args: CameraElevation (float): New elevation angle in degrees """ # Update the elevation angle self.CameraElevation = CameraElevation # Update the UI label to display the current elevation value self.ElevationLabel.setText("Camera Elevation: " + str(self.CameraElevation) + "°") # Refresh the camera position and orientation self.update_camera() def update_distance(self, CameraDistance): """ Update the camera's distance from the origin and refresh the camera view. This function sets the new camera distance value, updates the corresponding UI label, and triggers a camera update to apply the change. Args: CameraDistance (float): New distance value from the origin """ # Update the camera distance self.CameraDistance = CameraDistance # Update the UI label to display the current distance value self.DistanceLabel.setText("Camera Distance: " + str(self.CameraDistance)) # Refresh the camera position and orientation self.update_camera() def update_mode_toggle(self, mode): """ Update the UI to reflect the current operation mode. Changes the displayed mode label and switches the action widget to show the appropriate control set for the selected mode. Args: mode (int): Operation mode index 0 = Direct Control mode 1 = G-Code Generator mode """ # Switch the action widget to display the appropriate mode interface self.action_widget.setCurrentIndex(mode) # Update the mode label to reflect the current operating mode if mode == 0: # Direct Control mode self.mode_toggle_label.setText("Toggle Mode: Direct Control") self.GWOM.setChecked(False) self.GWOM.clicked.emit(self.GWOM.isChecked()) # Manually emit the signal self.close_all_editors() if mode == 1: # G-Code Generator mode self.mode_toggle_label.setText("Toggle Mode: G-Code Generator") self.SOM.setChecked(False) self.SOM.clicked.emit(self.SOM.isChecked()) self.open_gcode_editor(GCODETEMPFILE) def update_orio(self, Orio): """ Update the end effector orientation and refresh related displays and calculations. This function sets the new orientation value, updates the coordinate panel and orientation label, recalculates joint angles, and optionally sends commands to the robot if real-time sending is enabled. Args: Orio (float): New end effector orientation angle in degrees """ # Update the orientation value self.Orio = Orio # Update the coordinate panel with current position and orientation self.CoordinatesPanel.setText( "X = " + str(self.X) + "mm, Y = " + str(self.Y) + "mm, Z = " + str(self.z) + "mm" + "\n θ: " + str(self.Orio) + "°" ) # Update the dedicated orientation label self.OrientationLabel.setText("End Effector Orientation: " + str(self.Orio) + "°") # Recalculate joint angles without changing sphere appearance self.update_joint_angles(ChangeSphere=False) # Update coordinate display with current values self.update_coordinates_display(self.X, self.Y, self.z) # Send command to robot if real-time sending is enabled #if self.SendOnMove: # self.send_serial_to_robot() # Send command to robot if real-time sending is enabled # Rate-limited execution of send operation if self.SendOnMove: # Get current time for interval comparison current_time = time.time() # Check if sufficient time has passed since last send if current_time - self.last_write_time >= WRITE_INTERVAL: # Execute the send operation self.send_serial_to_robot() # Update last execution time to current time self.last_write_time = current_time # Rate-limited execution of write operation if self.write_on_move: # Get current time for interval comparison current_time = time.time() # Check if sufficient time has passed since last write if current_time - self.last_write_time >= WRITE_INTERVAL: # Execute the write operation self.write_pos_to_file() # Update last execution time to current time self.last_write_time = current_time def update_Z_orio(self, ZOrio): """ Update the robot's base Z-axis orientation and refresh related components. This function sets the new Z orientation value, updates the corresponding UI label, applies the rotation to the grid and first joint, calculates the new end effector position, updates the coordinate display, and optionally sends commands to the robot if real-time sending is enabled. Args: ZOrio (float): New base Z-axis orientation angle in degrees """ # Update the Z orientation value self.ZOrio = ZOrio # Update the Z orientation label in the UI self.ZOrioLabel.setText("Robot Z Orientation: " + str(self.ZOrio) + "°") # Apply rotation to the grid transform # Combines Z orientation with fixed offsets for proper alignment self.GridTransform.setRotation( QuartRotationXYZ(0, 0, self.ZOrio) * QuartRotationXYZ(0, 90, -90) ) # Apply rotation to the first joint transform # Combines joint angle, Z orientation, and position rotation self.Joint1Transform.setRotation( QuartRotationXYZ(self.Joint1Angle, 0, self.ZOrio - 90) * self.PosRot ) # Calculate the new end effector position after rotation changes pos3d = self.get_FinalJointPos() # Update coordinate display with rounded position values self.update_coordinates_display( round(pos3d.x(), 3), round(pos3d.y(), 3), round(pos3d.z(), 3) ) #if self.SendOnMove: # self.send_serial_to_robot() # Send command to robot if real-time sending is enabled # Rate-limited execution of send operation if self.SendOnMove: # Get current time for interval comparison current_time = time.time() # Check if sufficient time has passed since last send if current_time - self.last_write_time >= WRITE_INTERVAL: # Execute the send operation self.send_serial_to_robot() # Update last execution time to current time self.last_write_time = current_time # Rate-limited execution of write operation if self.write_on_move: # Get current time for interval comparison current_time = time.time() # Check if sufficient time has passed since last write if current_time - self.last_write_time >= WRITE_INTERVAL: # Execute the write operation self.write_pos_to_file() # Update last execution time to current time self.last_write_time = current_time def update_serial_on_move(self): """ Toggle the real-time serial sending mode. This function toggles the SendOnMove flag between True and False states, enabling or disabling the feature that sends commands to the robot automatically during movement operations. The toggle functionality: - If SendOnMove is True, sets it to False (disables real-time sending) - If SendOnMove is False, sets it to True (enables real-time sending) """ # Toggle the SendOnMove flag self.SendOnMove = self.SOM.isChecked() def update_vac_variable(self): """ Toggle the vacuum state and optionally send the updated state to the robot. This function toggles the vacuum state between on (True) and off (False). If real-time serial sending is enabled (SendOnMove is True), it immediately sends the updated vacuum state to the robot. The function: - Toggles the current vacuum state - Sends the updated state to the robot if real-time sending is enabled - Handles both turning the vacuum on and off Notes: - The vacuum state is used in the serial message sent to the robot - No visual feedback is provided in this function (commented print statements) """ # Toggle vacuum from ON to OFF if self.VacState: self.VacState = False # Send updated state to robot if real-time sending is enabled if self.SendOnMove: self.send_serial_to_robot() return # Toggle vacuum from OFF to ON if not self.VacState: self.VacState = True # Send updated state to robot if real-time sending is enabled if self.SendOnMove: self.send_serial_to_robot() return def update_write_on_move(self): """ Update the write-on-move flag based on the checkbox state. This function synchronizes the write_on_move boolean flag with the current checked state of the GWOM (Global Write On Move) checkbox. Operation: - Reads the checked state of the GWOM checkbox - Sets the write_on_move flag to match the checkbox state - If checkbox is checked: write_on_move = True (enabled) - If checkbox is unchecked: write_on_move = False (disabled) Notes: - Uses isChecked() which returns a boolean (True/False) - More appropriate than checkState() for simple on/off toggles - Provides immediate synchronization between UI and internal state """ # Synchronize internal flag with checkbox UI state self.write_on_move = self.GWOM.isChecked() # ============================================= # --- 3D-Scene-Updaters --- # ============================================= def update_camera(self): """ Update the camera position and orientation based on spherical coordinates. This function converts spherical coordinates (azimuth, elevation, distance) to Cartesian coordinates and positions the camera accordingly, looking at the origin (0, 0, 0) with Z-axis as the up vector. The camera uses a standard spherical coordinate system: - Azimuth: Angle in the XY plane (0° points along positive Y-axis) - Elevation: Angle above/below the XY plane - Distance: Radial distance from the origin Note: The function includes error handling to prevent crashes if the camera update fails for any reason. """ try: # Convert spherical coordinates to radians for trigonometric functions azimuth_rad = math.radians(self.CameraAzimuth) elevation_rad = math.radians(self.CameraElevation) # Convert spherical coordinates to Cartesian coordinates # Standard spherical to Cartesian conversion formulas: x = self.CameraDistance * math.cos(elevation_rad) * math.sin(azimuth_rad) y = self.CameraDistance * math.cos(elevation_rad) * math.cos(azimuth_rad) z = self.CameraDistance * math.sin(elevation_rad) # Get the camera object from the view camera = self.view.camera() # Set camera position in 3D space camera.setPosition(QVector3D(x, y, z)) # Set the point the camera is looking at (origin) camera.setViewCenter(QVector3D(0, 0, 0)) # Set the camera's up vector (Z-axis is up) camera.setUpVector(QVector3D(0, 0, 1)) except Exception: # Silently handle any exceptions to prevent application crashes # Camera updates failing shouldn't break the entire application pass def update_joint_angles(self, pos3d: QVector3D = None, ChangeSphere=True): """ Update the robot joint angles based on a target position or current end effector position. This function calculates the inverse kinematics for a given 3D position, updates the joint angles, applies the corresponding rotations to the joint transforms, and calculates the absolute end effector orientation. Args: pos3d (QVector3D, optional): Target 3D position for the end effector. If None, uses current end effector position. ChangeSphere (bool): Whether to update sphere visual indicators (default: True) Notes: Converts 3D position to 2D plane coordinates before inverse kinematics calculation Handles both direct position input and current position retrieval Updates joint transforms with calculated angles and Z orientation offset """ try: # Use current end effector position if no specific position is provided if pos3d is None: pos3d = self.get_FinalJointPos() # Convert 3D position to 2D plane coordinates x, y = self.to_plane_2d_coords(self.GridTransform, pos3d) # Calculate inverse kinematics with appropriate sphere update setting if ChangeSphere: theta1, theta2, theta3 = self.calculate_2D_inverse_kinematics(x, y, self.OrientationSlider.value()) else: theta1, theta2, theta3 = self.calculate_2D_inverse_kinematics(x, y, self.OrientationSlider.value(), ChangeSphere=False) # Update joint angle properties self.Joint1Angle = theta1 self.Joint2Angle = theta2 self.Joint3Angle = theta3 # Apply rotations to joint transforms # Joint1 includes Z orientation offset and position rotation self.Joint1Transform.setRotation(QuartRotationXYZ(self.Joint1Angle, 0, self.ZOrio - 90) * self.PosRot) # Joint2 and Joint3 use inverted angles with 180° offset self.Joint2Transform.setRotation(QuartRotationXYZ(-self.Joint2Angle + 180, 0, 0)) self.Joint3Transform.setRotation(QuartRotationXYZ(-self.Joint3Angle + 180, 0, 0)) # Calculate absolute end effector orientation AbsoluteTheta = theta1 + theta2 + theta3 - 360 self.Orio = round(AbsoluteTheta, 3) except Exception as e: print("An error occurred while updating joint angles:", e) # ============================================= # --- Computational Functions --- # -Functions that do math!! # ============================================= def calculate_2D_inverse_kinematics(self,x,y,end_effector_angle_deg,ChangeSphere=True): """ Calculate 2D inverse kinematics for a 3-joint planar robotic arm. Given the target end-effector position (x, y) and its orientation, this function calculates the angles for the three joints required to reach that position. Args: x (float): Target X-coordinate in mm. y (float): Target Y-coordinate in mm. end_effector_angle_deg (float): Desired end-effector orientation in degrees. ChangeSphere (bool, optional): Whether to visually update sphere and glow materials based on reachability. Default is True. Returns: tuple: (theta1_deg, theta2_deg, theta3_deg) - theta1_deg: Base joint angle (degrees) - theta2_deg: Elbow joint angle (degrees) - theta3_deg: Wrist joint angle (degrees) Raises: Exception: Any unexpected calculation error. """ try: # ============================================= # STEP 1 — Compute wrist center position # ============================================= # Subtract L3 offset to get wrist center (Xw, Yw) Xw = x - L3 * math.cos(math.radians(end_effector_angle_deg)) Yw = y - L3 * math.sin(math.radians(end_effector_angle_deg)) # Distance from base to wrist center r = math.sqrt(Xw**2 + Yw**2) # ============================================= # STEP 2 — Calculate theta2 (elbow angle) # ============================================= # Using the cosine rule: # cos(theta2) = (L1² + L2² - r²) / (2 * L1 * L2) theta2 = math.acos( max(-1.0, min(1.0, (L1**2 + L2**2 - r**2) / (2 * L1 * L2))) ) # Clamped to avoid NaN due to floating-point errors # ============================================= # STEP 3 — Calculate theta1 (base angle) # ============================================= # Using the law of cosines again for intermediate angle "b": a = math.atan2(Yw, Xw) # Angle from base to wrist center b = math.acos( max(-1.0, min(1.0, (r**2 + L1**2 - L2**2) / (2 * L1 * r))) ) # Clamped to avoid NaN theta1 = a + b # ============================================= # STEP 4 — Calculate theta3 (wrist angle) # ============================================= # theta3 = end-effector angle - theta1 - theta2 theta3 = math.radians(end_effector_angle_deg) - theta1 - theta2 # Ensure theta3 remains positive in degrees if math.degrees(theta3) < 0: theta3 = math.radians(math.degrees(theta3) + 360) # ============================================= # STEP 5 — Check joint angle limits # ============================================= # If all joint angles are within defined limits: if ( IsInRange(math.degrees(theta1), THETA2LIMIT) and IsInRange(math.degrees(theta2), THETA3LIMIT) and IsInRange(math.degrees(theta3), THETA4LIMIT) ): # If requested, update sphere material to indicate valid solution if ChangeSphere: self.Spherematerial.setAmbient(self.sphere_material_color_norm) self.Spherematerial.setDiffuse(self.sphere_material_color_norm) self.glow_material.setAmbient(self.glow_material_colorNorm) self.glow_material.setDiffuse(self.glow_material_colorNorm) # Return calculated angles in degrees return ( math.degrees(theta1), math.degrees(theta2), math.degrees(theta3) ) # ============================================= # STEP 6 — If out of range, update materials accordingly # ============================================= if ChangeSphere: self.Spherematerial.setAmbient(self.sphere_material_color_bad) self.Spherematerial.setDiffuse(self.sphere_material_color_bad) self.glow_material.setAmbient(self.glow_material_colorBad) self.glow_material.setDiffuse(self.glow_material_colorBad) # Return clamped angles to avoid mechanical violation return ( clamp_angle(math.degrees(theta1), THETA2LIMIT), clamp_angle(math.degrees(theta2), THETA3LIMIT), clamp_angle(math.degrees(theta3), THETA4LIMIT) ) except Exception as e: # Catch unexpected calculation errors for debugging print("An error occurred:", e) def to_plane_2d_coords(self, transform: QTransform, point3D: QVector3D): """ Convert a 3D point in world coordinates into 2D coordinates relative to a plane's local space. Args: transform (QTransform): The transform applied to the plane entity (position + rotation). point3D (QVector3D): The point in world coordinates to project onto the plane. Returns: tuple: (x, y) coordinates of the point in the plane's local 2D coordinate system. """ # ============================================= # STEP 1 — Extract plane origin & rotation # ============================================= origin = transform.translation() # Plane's position in world space rotation = transform.rotation() # Plane's orientation as a quaternion # ============================================= # STEP 2 — Get plane's local X and Y axes # ============================================= # Rotate the global X (1,0,0) and Y (0,1,0) vectors to align with plane's orientation local_x = rotation.rotatedVector(QVector3D(1, 0, 0)) local_y = rotation.rotatedVector(QVector3D(0, 1, 0)) # ============================================= # STEP 3 — Vector from plane origin to the 3D point # ============================================= vec = point3D - origin # ============================================= # STEP 4 — Project vector onto plane's local axes # ============================================= # Use dot product to project onto rotated X and Y directions y = QVector3D.dotProduct(vec, local_x.normalized()) # Component along plane's X-axis x = QVector3D.dotProduct(vec, local_y.normalized()) # Component along plane's Y-axis # ============================================= # STEP 5 — Store computed plane coordinates # ============================================= self.planeX = x self.planeY = -y # Invert Y-axis for plane's local convention # ============================================= # STEP 6 — Return computed local 2D coordinates # ============================================= return x, -y # ============================================= # --- 3D-Scene Event Handlers --- # ============================================= def on_plane_click(self, pick: QPickEvent): """ Handle click events on the plane grid. This function is called when the user clicks on the grid plane in the 3D scene. It places a sphere at the clicked position, updates joint angles, and initiates dragging mode. Args: pick (QPickEvent): The pick event containing information about the click interaction """ # Get the 3D world coordinates of the click intersection pos3d = pick.worldIntersection() # Spawn a visual sphere at the clicked position self.spawn_sphere_at(pos3d) # Update joint angles based on the clicked position self.update_joint_angles(pos3d) # Get the final joint position (end effector position) pos3d = self.get_FinalJointPos() # Update the coordinate display with the end effector position self.update_coordinates_display(pos3d.x(), pos3d.y(), pos3d.z()) # Enable dragging mode for subsequent interactions self.dragging = True # Make the grid plane fully opaque during interaction self.planeGridMaterial.setAlpha(1) def on_mouse_move(self, pick: QPickEvent): """ Handle mouse movement events during dragging operations. This function updates the sphere position and joint angles when the user drags the sphere across the grid plane. Args: pick (QPickEvent): The pick event containing information about the mouse movement """ # Only process movement if currently dragging and a sphere exists if self.dragging and self.sphere_entity is not None: # Get the 3D world coordinates of the current mouse position pos3d = pick.worldIntersection() # Update the sphere's position to follow the mouse self.SphereTransform.setTranslation(pos3d) # Update joint angles based on the new position self.update_joint_angles(pos3d) # Get the final joint position (end effector position) pos3d = self.get_FinalJointPos() # Update the coordinate display with the new end effector position self.update_coordinates_display(pos3d.x(), pos3d.y(), pos3d.z()) # Rate-limited execution of write operation if self.write_on_move: # Get current time for interval comparison current_time = time.time() # Check if sufficient time has passed since last write if current_time - self.last_write_time >= WRITE_INTERVAL: # Execute the write operation self.write_pos_to_file() # Update last execution time to current time self.last_write_time = current_time def on_mouse_release(self, pick: QPickEvent): """ Handle mouse release events to end dragging operations. This function cleans up after dragging is completed, resets visual states, and optionally sends final commands to the robot. Args: pick (QPickEvent): The pick event containing information about the mouse release """ # End dragging mode self.dragging = False # Restore grid plane transparency to semi-transparent state self.planeGridMaterial.setAlpha(0.5) # Remove the sphere entity if it exists if hasattr(self, 'sphere_entity') and self.sphere_entity is not None: self.sphere_entity.setParent(None) # Detach from scene self.sphere_entity = None # Send final command to robot if real-time sending is enabled if self.SendOnMove: self.send_serial_to_robot() # ============================================= # --- Serial Communication Section --- # ============================================= def serial_connection_robot(self): """ Toggle serial connection to the robot arm. This function handles establishing and closing the serial connection to the physical robot. It toggles between connected and disconnected states, updates the UI button appearance, and provides error handling for serial communication issues. The function manages: - Serial port connection/disconnection - UI button text and styling changes - Error handling for serial exceptions """ # Connect if no serial connection exists if self.ser is None: try: # Establish serial connection self.ser = serial.Serial(SERIAL_PORT, BAUD_RATE) # Update button to show disconnect state self.CTS.setText("Disconnect") self.CTS.setStyleSheet(self.stylesheet_red_button) return except serial.SerialException as e: print(f"Error connecting to serial port: {e}") return # Handle existing serial connection if self.ser: # Close connection if currently open if self.ser.is_open: try: self.ser.close() # Update button to show connect state self.CTS.setText("Connect to RobotArm") self.CTS.setStyleSheet(self.stylesheet_connect_to_serial_button) return except serial.SerialException as e: print(f"Error closing serial port: {e}") return # Reopen connection if currently closed if not self.ser.is_open: try: self.ser.open() # Update button to show disconnect state self.CTS.setText("Disconnect") self.CTS.setStyleSheet(self.stylesheet_red_button) return except serial.SerialException as e: print(f"Error opening serial port: {e}") return def print_serial_to_terminal(self): """ Read and print incoming data from the robot arm via serial communication. This function continuously reads available data from the serial buffer while data is present, decodes it, and prints it to the console for monitoring and debugging purposes. The function: - Checks if serial connection is available and open - Reads all available data in the input buffer - Strips whitespace from received data - Prints non-empty messages to the console - Handles serial communication errors gracefully Error Handling: - Returns early if no valid serial connection exists - Catches and reports serial read errors without crashing - Uses error='ignore' to handle decoding issues gracefully """ # Check if serial connection is available and open if self.ser is None or not self.ser.is_open: return # Process all available data in the serial buffer while self.ser.in_waiting: # Ensure there is data to read if self.ser.in_waiting > 0: try: # Read and decode available data, ignoring decoding errors data = self.ser.read(self.ser.in_waiting).decode(errors='ignore') # Clean up the received data line = data.strip() # Print non-empty messages to console if line: print("Received:", line) except serial.SerialException as e: print(f"Error reading data from robot arm: {e}") def InitializeSerialTimer(self): """ Initialize and start a timer for periodic serial communication monitoring. This function creates a QTimer that periodically checks for incoming data from the robot arm via the serial connection. The timer triggers the ReadPrintSerial function at regular intervals to process any received data. The timer: - Runs continuously with a 10ms interval (100 Hz) - Connects to the ReadPrintSerial method for data processing - Is stored as an instance variable for later management Notes: - The interval can be adjusted based on performance requirements - The timer is automatically stopped if the parent object is destroyed """ # Create a QTimer instance self.serial_timer = QTimer(self) # Connect the timer timeout signal to the serial reading function self.serial_timer.timeout.connect(self.print_serial_to_terminal) # Start the timer with a 10ms interval (100 times per second) self.serial_timer.start(10) # Adjust the interval as needed def send_serial_to_robot(self): """ Send current robot state data to the physical robot arm via serial communication. This function formats the current position, orientation, and vacuum state into a message string and sends it to the connected robot arm. The message includes start and end delimiters for packet framing. The message format is: ,{StartDigit},{X},{Y},{Z},{-Angle},{VacuumState},{EndDigit}, Requirements: - Serial connection must be established and open - Robot must be in a valid state to receive commands Error Handling: - Catches and reports serial communication errors - Does nothing if serial connection is not available or open """ # Check if serial connection exists and is open if self.ser and self.ser.is_open: try: # Get current robot state values x = self.X y = self.Y z = self.z angle = self.Orio vac = self.VacState f = 100 EndDigit = 142 StartDigit = 142 # Format message with position, orientation, and vacuum state # Note: Angle is negated to match robot's coordinate system #msg = f",{StartDigit},{x:.3f},{y:.3f},{z:.3f},{angle:.2f},{int(vac)},{EndDigit}," msg = f"G01 X{x:.3f} Y{y:.3f} Z{z:.3f} A{angle:.3f} F{f:.3f} \n" # Print message for debugging purposes #print(msg) # Send encoded message via serial connection self.ser.write(msg.encode()) if vac is True: msg = "M03\n" else: msg = "M05\n" # Print message for debugging purposes #print(msg) self.ser.write(msg.encode()) except serial.SerialException as e: print(f"Error sending data to robot arm: {e}") def send_gcode_file(self): # Check if already running if hasattr(self, 'sender_thread') and self.sender_thread.isRunning(): self.show_error("⚠️ G-code sending already in progress!") return if not self.ser or not self.ser.is_open: self.show_error("Serial connection not established") return # Create and start thread self.serial_timer.stop() self.editor.AlterWidget.setCurrentIndex(1) self.editor.text_edit.setReadOnly(True) self.sender_thread = GCodeSenderThread(self.ser, GCODETEMPFILE, LINES_PER_BATCH) self.sender_thread.progress_signal.connect(self.update_gcode_progress) self.sender_thread.progress_value.connect(self.update_progress_bar) self.sender_thread.finished_signal.connect(self.on_gcode_finished) self.sender_thread.error_signal.connect(self.show_error) self.sender_thread.start() def update_gcode_progress(self, message): self.editor.text_edit.setPlainText(self.editor.text_edit.toPlainText() + "\n" + message) self.editor.scroll_to_bottom() def update_progress_bar(self, value): """Update progress bar value""" self.editor.progress_bar.setValue(value) def on_gcode_finished(self): self.editor.text_edit.setReadOnly(False) self.editor.AlterWidget.setCurrentIndex(0) self.editor.load_file() self.serial_timer.start() def show_error(self, error): self.editor.text_edit.setPlainText(self.editor.text_edit.toPlainText() + "\n" + error) self.editor.scroll_to_bottom() def closeEvent(self, event): # Stop thread when closing if hasattr(self, 'sender_thread') and self.sender_thread.isRunning(): self.sender_thread.stop() self.sender_thread.wait() event.accept() # ============================================= # --- G-Code Generation --- # ============================================= def generate_G01_command(self, x=None, y=None, z=None, A=None, B=None, C=None, F=None): """ Generate a G01 (linear interpolation) G-code command string with specified parameters. This function constructs a G01 command line by including only the parameters that are provided (not None). The G01 command moves the tool in a straight line at the specified feed rate to the given coordinates. Args: x (float, optional): X-axis coordinate y (float, optional): Y-axis coordinate z (float, optional): Z-axis coordinate A (float, optional): A-axis (rotational) coordinate B (float, optional): B-axis (rotational) coordinate C (float, optional): C-axis (rotational) coordinate F (float, optional): Feed rate Returns: str: A formatted G01 G-code command string with only the specified parameters Example: write_to_g_code_file_G01(x=10.5, y=20.0, F=300) Returns: "G01 X10.5 Y20.0 F300" """ # G01 is the G-code for linear interpolation (straight line movement) GCODETYPE = "G01" # Dictionary mapping parameter indices to G-code axis letters g_code_dict = { 1: "X", # X-axis 2: "Y", # Y-axis 3: "Z", # Z-axis 4: "A", # A-axis (typically rotational) 5: "B", # B-axis (typically rotational) 6: "C", # C-axis (typically rotational) 7: "F" # Feed rate } # List of all input parameters in the order they correspond to g_code_dict data_list = [x, y, z, A, B, C, F] # Initialize the G-code line with the command type gcode_line = GCODETYPE # Iterate through parameters and add non-None values to the G-code line for index, data in enumerate(data_list, 1): if data is not None: # Format the parameter as "LetterValue" (e.g., "X10.5") param = f"{g_code_dict[index]}{str(data)}" # Add the parameter to the G-code line with a space separator gcode_line += " " + param return gcode_line def write_to_file(self, command): """ Append a command to the G-code temporary file. This function opens the global G-code temporary file in append mode and writes the provided command followed by a newline character. Args: command (str): The G-code command string to append to the file Notes: - Uses append mode ('a') to preserve existing file content - Automatically adds a newline after each command - Relies on GCODETEMPFILE global constant for filename - Uses context manager for automatic file closing and error handling """ # Open file in append mode (creates file if it doesn't exist) with open(GCODETEMPFILE, 'a') as file: # Write command followed by newline for proper formatting file.write(command + "\n") def erase_line(self): """ Remove the last line from the G-code temporary file. This function reads the entire file, removes the last line, and writes the remaining content back to the file. It handles empty files gracefully by returning early if no content exists. Steps: 1. Open file in read/write mode 2. Read all lines into a list 3. Check if file is empty (return if true) 4. Remove the last line from the list 5. Rewind to file beginning 6. Write remaining lines back to file 7. Truncate to ensure no leftover content remains Notes: - Uses 'r+' mode for reading and writing in single operation - file.truncate() is crucial to remove any leftover content - Handles empty files without error """ # Open file in read/write mode with open(GCODETEMPFILE, 'r+') as file: # Read all lines from the file list_lines = file.readlines() # Return early if file is empty if not list_lines: return # Remove the last line list_lines = list_lines[:-1] # Move to beginning of file for overwriting file.seek(0) # Write remaining lines back to file file.writelines(list_lines) # Truncate to remove any leftover content from original file file.truncate() def erase_all_lines(self): """ Completely erase all content from the G-code temporary file. This function clears the entire file by opening it in write mode ('w'), which automatically truncates the file to zero length, effectively deleting all existing content. Operation: - Opens the file in write mode ('w') - The 'w' mode automatically: - Truncates the file to 0 bytes (erases all content) - Creates the file if it doesn't exist - No explicit write operation is needed Notes: - This is the most efficient way to clear a file - Uses context manager for automatic file closing - Much faster than reading and processing content - Preserves file existence and metadata while removing content """ # Open file in write mode - automatically truncates to 0 bytes with open(GCODETEMPFILE, 'w') as file: pass # No operation needed - file is already cleared by 'w' mode def write_pos_to_file(self): """ Write the current robot position as a G01 command to the G-code file. This function generates a G01 (linear interpolation) command using the current robot coordinates and orientation, then appends it to the G-code temporary file. Steps: 1. Generates a G01 command with current position values: - X, Y, Z coordinates from current state - A-axis (orientation) from current Orio value 2. Writes the generated G-code command to the file Uses: - self.generate_G01_command(): Creates the formatted G-code string - self.write_to_file(): Appends the command to the file Notes: - G01 is for linear movement (straight line) - Maintains the current orientation during movement - Appends to file rather than overwriting existing content """ # Generate G01 command with current position and orientation code = self.generate_G01_command(x=self.X, y=self.Y, z=self.z, A=self.Orio) # Append the generated command to the G-code file self.write_to_file(code) def write_pause_to_file(self): """ Prompt user for pause time and write corresponding G-code to file. Displays an input dialog to get pause time in milliseconds, then generates G04 pause command and writes it to the output file. The G04 command format is: "G04 P[time_in_ms]" """ dialog = QInputDialog(self) # Configure dialog properties dialog.setWindowTitle("Write Pause Time") dialog.setLabelText("Enter pause time (ms):") dialog.setInputMode(QInputDialog.IntInput) dialog.setSizePolicy(QSizePolicy.Policy.Minimum, QSizePolicy.Policy.Minimum) # Set input constraints dialog.setIntMinimum(0) # Minimum value (0 milliseconds) dialog.setIntMaximum(2147483647) # Maximum value (max 32-bit signed integer) dialog.setIntStep(1) # Step size # Apply styling dialog.setStyleSheet(""" color: #bbbbbb; border: 4px solid #626262; border-radius: 4px; padding: 2px; """) # Execute dialog and process result if dialog.exec() == QInputDialog.DialogCode.Accepted: pause_time = dialog.intValue() g_code = f"G04 P{pause_time}" self.write_to_file(g_code) def open_gcode_editor(self, file): """Open a non-modal G-code editor window""" self.editor = GCodeEditor(self, file) self.editor.show() self.gcode_editors.append(self.editor) # ✅ CONNECT THE SIGNAL HERE self.editor.editor_closed.connect(self.switch_to_direct_control_on_close) self.editor.send_gcode_file.clicked.connect(self.send_gcode_file) # Clean up reference when editor closes self.editor.destroyed.connect(lambda: self.gcode_editors.remove(self.editor)) def switch_to_direct_control_on_close(self): self.mode_toggle.setValue(0) def on_gcode_file_updated(self): """Update all open editors when file changes""" for editor in self.gcode_editors: if editor.isVisible() and not editor.text_edit.document().isModified(): editor.load_file() def close_all_editors(self, force=False): """ Close all open G-code editor windows with optional force close. Args: force (bool): If True, bypass save prompts and close immediately If False, allow editors to prompt for unsaved changes Operation: - If force=True: Uses hide() and deleteLater() for immediate closure - If force=False: Uses normal close() with save prompts - Always clears the tracking list regardless of method """ for editor in self.gcode_editors: if force: # Immediate closure without prompts editor.hide() editor.deleteLater() # Schedule for Qt cleanup else: # Normal closure with save prompts editor.close() # Triggers closeEvent # Clear tracking list to prevent stale references self.gcode_editors.clear() # ============================================= # --- Miscellaneous Helper Functions --- # ============================================= def get_FinalJointPos(self): """ Calculate the 3D position of the end effector (final joint) in world coordinates. This function computes the cumulative transformation matrix by multiplying all joint and segment transformation matrices in the kinematic chain, then applies this transformation to the origin to get the end effector position. The kinematic chain order is: Joint1 → Segment1 → Joint2 → Segment2 → Joint3 → Segment3 → Joint4 Returns: QVector3D: The 3D world coordinates of the end effector position """ # Initialize transformation matrix Matrix = QMatrix4x4() # Multiply all transformation matrices in the kinematic chain Matrix = (self.Joint1Transform.matrix() * self.Segment1Transform.matrix() * self.Joint2Transform.matrix() * self.Segment2Transform.matrix() * self.Joint3Transform.matrix() * self.Segment3Transform.matrix() * self.Joint4Transform.matrix()) # Transform the origin point (0, 0, 0) through the cumulative matrix # to get the end effector position in world coordinates pos3d = Matrix.map(QVector3D(0, 0, 0)) return pos3d def update_coordinates_display(self, x, y, z): """ Update the coordinate display panel with current position and orientation values. This function rounds the input coordinates to 3 decimal places for display, stores them as instance variables, and updates the text in the coordinates panel to show the current position and end effector orientation. Args: x (float): X-coordinate value in millimeters y (float): Y-coordinate value in millimeters z (float): Z-coordinate value in millimeters The display format is: "X = {x}mm, Y = {y}mm, Z = {z}mm" " θ: {orientation}°" """ # Round and store coordinate values self.X = round(x, 3) self.Y = round(y, 3) self.z = round(z, 3) # Update the coordinates panel text with formatted values self.CoordinatesPanel.setText( "X = " + str(self.X) + "mm, Y = " + str(self.Y) + "mm, Z = " + str(self.z) + "mm" + "\n θ: " + str(self.Orio) + "°" ) def main(): """ Main entry point for the Robot 3D Simulator application. This function: - Creates the QApplication instance (required for any Qt application) - Initializes the main application window (Robot3DSimulator) - Displays the main window - Enters the Qt event loop for handling user interactions The application will run until the main window is closed or sys.exit() is called. """ # Create the Qt application instance app = QApplication(sys.argv) # Create and initialize the main application window window = Robot3DSimulator() # Display the main window window.show() # Enter the Qt event loop and exit when done sys.exit(app.exec_()) if __name__ == "__main__": # Execute the main function when script is run directly main()