Added Gui (unfinished)

config into dataclass and enums
new Gui that includes settings
deleted GlobalVariables

small fixes (import, names...)

.gitignore

@@ -1,7 +1,7 @@
 # Ignore dependency folders
 node_modules/
 .venv/
-.vs/
+.vscode/
 __pycache__/
 ml/checkpoints/
 ml/tensorboard/

ArduinoCommunication.py

@@ -4,13 +4,13 @@ import numpy as np
 import serial
 import time
 
-import config as cfg
+from config import cfg
 import DataTypes as dt
 
 class ArduinoCommunication(Thread):
-    def __init__(self, port=cfg.port, baudrate=cfg.baudrate, timeout=cfg.timeout):
+    def __init__(self, port=cfg.port, baudrate=cfg.baudrate, timeout=cfg.comm_timeout):
         super().__init__()
-        self.daemon = True  # Thread schließt sich beim Programmende
+        self.daemon = True  # Thread schlie�t sich beim Programmende
         self.serial_conn  = serial.Serial(port, baudrate, timeout=timeout)
         time.sleep(2)  # Warten bis Arduino ready
 

EspCommunication.py

@@ -5,7 +5,7 @@ import numpy as np
 from threading import Thread, Event
 from queue import Queue
 
-import config as cfg
+from config import cfg
 import DataTypes as dt
 
 # ============================================================

GlobalVariables.py

@@ -1,39 +0,0 @@
-import numpy as np
-from ArduinoCommunication import ArduinoCommunication
-from EspCommunication import ESP32Communication
-from simulation import Simulation
-import DataTypes as dt
-import config as cfg
-from robot_init import init_deg, init90_deg, init_pos, center_points
-
-# Globale Variablen
-robotCommunication = None
-shared_sim = None
-emote = None
-display_text = ""
-
-if cfg.sim:
-    shared_sim = Simulation()
-else:
-    if cfg.arduinoConnection:
-        robotCommunication = ArduinoCommunication()
-    else:
-        robotCommunication = ESP32Communication()
-
-vector_dirmov = [0, 0, 0]  # Direction Movement [vx, vy, omega]
-current_rad: dt.RadArray  # Current Rad
-current_pos: dt.PosArray  # Current Position
-
-# current_deg: dt.DegArray  # Current Degrees
-# legarray: dt.DegArray  # Working Leg Array
-# target_pos: dt.PosArray  # Target Position
-
-robot_state = "idle"  # Current State
-# Init for 6 Legs
-#leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
-# Init for 4 Legs
-leg_state = np.array(["step", "drag", "drag", "drag", "drag", "drag"])
-control_pause: bool = False
-
-# Initilize mit Start Position
-## Shared init pose definitions are imported from robot_init.py

Robot.py

@@ -1,9 +1,9 @@
 """
-robot.py - Unified Robot Class for JackBot
+Robot.py - Unified Robot Class for JackBot
 Handles state, kinematics, backends (Hardware/Simulation), and motion execution.
 """
 
-from typing import Protocol, Optional
+from typing import Protocol, Optional, Union
 import numpy as np
 import math
 
@@ -12,7 +12,12 @@ from states.State import State
 import DataTypes as dt
 import kinematics as kin
 import robot_init as ri
-import config as cfg
+from config import cfg, BackendType
+
+# Import communications and simulation modules
+from simulation import Simulation
+from EspCommunication import ESP32Communication
+from ArduinoCommunication import ArduinoCommunication
 
 
 class RobotBackend(Protocol):
@@ -21,6 +26,8 @@ class RobotBackend(Protocol):
         ...
     def step_simulation(self) -> None:
         ...
+    def cleanup(self) -> None:
+        ...
 
 
 class HardwareBackend:
@@ -35,12 +42,15 @@ class HardwareBackend:
     def step_simulation(self) -> None:
         pass  # Physical hardware steps in real-time
 
+    def cleanup(self) -> None:
+        if self.comm_channel and hasattr(self.comm_channel, 'close'):
+            self.comm_channel.close()
+
 
 class PyBulletBackend:
     """Backend for PyBullet simulation execution."""
-    def __init__(self, sim_instance, body_id: int = 0):
+    def __init__(self, sim_instance):
         self.sim = sim_instance
-        self.body_id = body_id
 
     def send_angles(self, rad_array: dt.RadArray) -> None:
         if self.sim:
@@ -50,6 +60,10 @@ class PyBulletBackend:
         if self.sim:
             self.sim.step()
 
+    def cleanup(self) -> None:
+        if self.sim and hasattr(self.sim, 'close'):
+            self.sim.close()
+
 
 class Robot:
     """
@@ -59,15 +73,28 @@ class Robot:
 
     def __init__(
         self,
-        backend: Optional[RobotBackend] = None,
+        backend_type: Union[BackendType, RobotBackend] = BackendType.SIMULATION,
         start_pose: str = "init_deg",
         urdf_path: str = cfg.urdf_path
     ):
-        # 1. Store configuration & backend FIRST
-        self.backend = backend
         self.urdf_path = urdf_path
 
-        # 2. Initialize kinematics and position data
+        # --- BACKEND FACTORY CREATION ---
+        if isinstance(backend_type, BackendType):
+            if backend_type == BackendType.SIMULATION:
+                # Launch PyBullet 3D Simulation GUI
+                sim_instance = Simulation(urdf_path=self.urdf_path)
+                self.backend: RobotBackend = PyBulletBackend(sim_instance)
+            elif backend_type == BackendType.ESP32:
+                comm = ESP32Communication(ip=cfg.esp32_ip, port=cfg.esp32_port)
+                self.backend = HardwareBackend(comm)
+            elif backend_type == BackendType.ARDUINO:
+                comm = ArduinoCommunication(port=cfg.port, baudrate=cfg.baudrate)
+                self.backend = HardwareBackend(comm)
+        else:
+            self.backend = backend_type
+
+        # Kinematics and position initialization
         pose_deg = ri.init_deg if start_pose == "init_deg" else ri.init90_deg
         self.current_rad: dt.RadArray = pose_deg.to_rad()
         self.current_pos: dt.PosArray = kin.ikpyForward(self.current_rad)
@@ -75,12 +102,12 @@ class Robot:
             ri.get_center_points() if hasattr(ri, "get_center_points") else ri.center_points
         )
 
-        # 3. Initialize gait / motion variables
+        # Gait / motion variables
         self.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
         self.robot_state = "idle"
         self.vector_dirmov = [0.0, 0.0, 0.0]  # [vx, vy, omega]
 
-        # 4. Set state and trigger enter() LAST
+        # State machine initialization
         self.current_state_key: str = "idle"
         self.current_state: State = STATE_REGISTRY["idle"]
         self.current_state.enter(self)
@@ -96,68 +123,28 @@ class Robot:
             self.current_state.execute(self)
         self.step_sim()
 
-
-    # -------------------------------------------------------------------------
-    # Core Motion Execution
-    # -------------------------------------------------------------------------
-
     def set_joint_angles(self, target_rad: dt.RadArray) -> None:
-        """Applies joint angles to internal state and sends to the active backend."""
         self.current_rad = target_rad
-        
         if self.backend:
             self.backend.send_angles(target_rad)
 
     def step_sim(self) -> None:
-        """Advances physics simulation step if applicable."""
         if self.backend:
             self.backend.step_simulation()
 
     def reset_to_init(self) -> None:
-        """Resets the robot to its default standing stance."""
         self.current_rad = ri.init_deg.to_rad()
         self.current_pos = kin.ikpyForward(self.current_rad)
         self.set_joint_angles(self.current_rad)
         self.step_sim()
 
-    # -------------------------------------------------------------------------
-    # Inverse / Forward Kinematics wrappers bound to this instance
-    # -------------------------------------------------------------------------
-
     def compute_ik(self, target_pos: dt.PosArray) -> dt.RadArray:
-        """
-        Computes IK using this specific robot instance's current joint angles 
-        as the seed initial guess.
-        """
         return kin.ikpyInverse(target_pos, initial_rad=self.current_rad)
 
     def compute_fk(self, target_rad: Optional[dt.RadArray] = None) -> dt.PosArray:
-        """Computes Forward Kinematics for joint angles."""
         rads = target_rad if target_rad is not None else self.current_rad
         return kin.ikpyForward(rads)
 
-    # -------------------------------------------------------------------------
-    # Machine Learning / RL Helper Methods
-    # -------------------------------------------------------------------------
-
-    def get_observation(self, command: np.ndarray) -> np.ndarray:
-        """
-        Returns flat observation vector [joint_angles (18,), command (4,)]
-        for reinforcement learning input.
-        """
-        joint_flat = self.current_rad.data.flatten()
-        return np.concatenate([joint_flat, command]).astype(np.float32)
-
-    def apply_rl_action(self, action_delta: np.ndarray, scale: float = math.radians(8.0)) -> dt.RadArray:
-        """
-        Applies continuous angle deltas from an RL policy network.
-        """
-        current_flat = self.current_rad.data.flatten()
-        new_flat = current_flat + action_delta * scale
-        new_rad = dt.RadArray(new_flat.reshape(6, 3))
-        self.set_joint_angles(new_rad)
-        return new_rad
-
     def transition_to(self, next_state_key: str) -> None:
         if next_state_key in STATE_REGISTRY and next_state_key != self.current_state_key:
             self.current_state.exit(self)
@@ -166,8 +153,11 @@ class Robot:
             self.current_state.enter(self)
 
     def tick(self) -> None:
-        """Executes one step of the current active state."""
         next_state_key = self.current_state.execute(self)
         if next_state_key:
             self.transition_to(next_state_key)
         self.step_sim()
+
+    def cleanup(self) -> None:
+        if self.backend and hasattr(self.backend, 'cleanup'):
+            self.backend.cleanup()

config.py

@@ -1,31 +1,47 @@
-# Connection
+"""
-port = "COM4"
+config.py - Central Configuration & Parameters for JackBot
-baudrate = 38400
+"""
+from dataclasses import dataclass, field
+from enum import Enum
 
-esp32_ip = "192.168.188.32"
-esp32_port = 3323
 
-timeout = 2.0
+class BackendType(Enum):
+    SIMULATION = "sim"
+    ESP32 = "esp32"
+    ARDUINO = "arduino"
 
-sim: bool = True
-arduinoConnection: bool = False
-urdf_path = "JackBotUrdf.urdf"
 
-# row = legnumber (left 0,1,2 right 3,4,5)
+@dataclass
-# column = servo position from torso(0) to feet(2)
+class RobotConfig:
+    # -------------------------------------------------------------------------
+    # 1. Execution & Backend Target
+    # -------------------------------------------------------------------------
+    backend: BackendType = BackendType.SIMULATION
+    urdf_path: str = "JackBotUrdf.urdf"
 
-# Dimensions in m
+    # Hardware Connection Settings
-#robot_height = -0.1
+    port: str = "COM4"                  # Serial Port for Arduino
-step_height = 0.03
+    baudrate: int = 38400               # Baudrate for Serial
-step_length = 0.04
+    esp32_ip: str = "192.168.188.32"   # Wi-Fi IP for ESP32 UDP communication
-translation_gain = 1.0
+    esp32_port: int = 3323              # UDP Target Port
-rotation_gain = 2.0
+    comm_timeout: float = 2.0           # Connection timeout threshold [s]
-# Leglength in m
-# L1 = 0.068
-# L2 = 0.0602
-# L3 = 0.070
 
-# standard_tickpersec = # 20 Fluessige Bewegung
+    # -------------------------------------------------------------------------
-standard_tickpersec:float = 25 # [ticks/s]
+    # 2. Gait & Motion Kinematics Parameters
-standard_duration:float = 0.8  # [s]
+    # -------------------------------------------------------------------------
-standard_tickduration:float = 1 / standard_tickpersec
+    step_height: float = 0.05          # Clearance height of foot swing [m]
+    step_length: float = 0.08          # Maximum stride distance [m]
+    translation_gain: float = 1.0      # Speed multiplier for directional translation
+    rotation_gain: float = 4.0         # Speed multiplier for yaw rotation
+
+    # Timing & Execution Frequency
+    tick_rate_hz: float = 25.0         # Motion loop execution rate [ticks/sec]
+    step_duration: float = 0.8         # Time to complete full gait stride [s]
+
+    @property
+    def tick_duration(self) -> float:
+        return 1.0 / self.tick_rate_hz
+
+
+# Global Active Instance
+cfg = RobotConfig()

gui/MainWindow.py

@@ -0,0 +1,184 @@
+"""
+MainWindow.py - Master Dashboard with Dynamic Input Selection
+"""
+import random
+import time
+import dearpygui.dearpygui as dpg
+from config import cfg, BackendType
+
+# Input State Tracking
+input_config = {
+    "source": "Gamepad",  # Options: "Gamepad", "GUI Sliders", "Random Walk"
+    "manual_vx": 0.0,
+    "manual_vy": 0.0,
+    "manual_omega": 0.0,
+    "random_interval": 2.0,  # Change random vector every N seconds
+    "last_random_time": 0.0,
+    "random_vx": 0.0,
+    "random_vy": 0.0,
+    "random_omega": 0.0,
+}
+
+
+def create_parameter_gui(on_start_callback=None, on_stop_callback=None):
+    dpg.create_context()
+    dpg.create_viewport(title="JackBot Master Control Deck", width=460, height=800)
+
+    is_running = {"value": False}
+
+    def toggle_start(sender, app_data, user_data):
+        is_running["value"] = not is_running["value"]
+        if is_running["value"]:
+            dpg.configure_item("start_btn", label="STOP ROBOT")
+            dpg.configure_item("status_text", default_value="Status: RUNNING", color=(0, 255, 0))
+            dpg.configure_item("backend_combo", enabled=False)
+            if on_start_callback:
+                on_start_callback()
+        else:
+            dpg.configure_item("start_btn", label="START ROBOT")
+            dpg.configure_item("status_text", default_value="Status: STOPPED", color=(255, 100, 100))
+            dpg.configure_item("backend_combo", enabled=True)
+            if on_stop_callback:
+                on_stop_callback()
+
+    with dpg.window(label="Robot Control & Telemetry Deck", width=440, height=770, pos=(5, 5)):
+        
+        # --- Execution Control ---
+        dpg.add_text("Control State", color=(0, 200, 255))
+        dpg.add_text("Status: STOPPED", tag="status_text", color=(255, 100, 100))
+        dpg.add_button(
+            label="START ROBOT", 
+            tag="start_btn", 
+            width=-1, 
+            height=40, 
+            callback=toggle_start
+        )
+        dpg.add_separator()
+
+        # --- Input Source Switcher ---
+        dpg.add_text("Input Controller Source", color=(0, 200, 255))
+        dpg.add_combo(
+            tag="input_source_combo",
+            label="Input Source",
+            items=["Gamepad", "GUI Sliders", "Random Walk"],
+            default_value=input_config["source"],
+            callback=lambda s, d: input_config.update({"source": d})
+        )
+
+        # Manual Sliders (Active when GUI Sliders selected)
+        dpg.add_slider_float(
+            label="Vx (Fwd/Bwd)",
+            default_value=0.0, min_value=-1.0, max_value=1.0,
+            callback=lambda s, d: input_config.update({"manual_vx": d})
+        )
+        dpg.add_slider_float(
+            label="Vy (Strafe)",
+            default_value=0.0, min_value=-1.0, max_value=1.0,
+            callback=lambda s, d: input_config.update({"manual_vy": d})
+        )
+        dpg.add_slider_float(
+            label="Omega (Turn)",
+            default_value=0.0, min_value=-1.0, max_value=1.0,
+            callback=lambda s, d: input_config.update({"manual_omega": d})
+        )
+        dpg.add_separator()
+
+        # --- Active Command Readouts ---
+        dpg.add_text("Active Command Output", color=(0, 200, 255))
+        dpg.add_text("Joysticks Connected: 0", tag="txt_joy_count")
+        dpg.add_text("Motion State: IDLE", tag="txt_motion_state", color=(200, 200, 200))
+        dpg.add_text("Active Vx   :  0.00", tag="txt_vx")
+        dpg.add_text("Active Vy   :  0.00", tag="txt_vy")
+        dpg.add_text("Active Omega:  0.00", tag="txt_omega")
+        dpg.add_separator()
+
+        # --- Backend Selection ---
+        dpg.add_text("Backend Selection", color=(0, 200, 255))
+        dpg.add_combo(
+            tag="backend_combo",
+            label="Target Backend",
+            items=[e.value for e in BackendType],
+            default_value=cfg.backend.value,
+            callback=lambda sender, data: setattr(cfg, 'backend', BackendType(data))
+        )
+        dpg.add_separator()
+
+        # --- Kinematics Adjustments ---
+        dpg.add_text("Gait & Kinematics", color=(0, 200, 255))
+        dpg.add_slider_float(
+            label="Step Height (m)",
+            default_value=cfg.step_height,
+            min_value=0.01, max_value=0.10, format="%.3f m",
+            callback=lambda sender, data: setattr(cfg, 'step_height', data)
+        )
+        dpg.add_slider_float(
+            label="Step Length (m)",
+            default_value=cfg.step_length,
+            min_value=0.01, max_value=0.10, format="%.3f m",
+            callback=lambda sender, data: setattr(cfg, 'step_length', data)
+        )
+        dpg.add_separator()
+
+        # --- Hardware Comms ---
+        dpg.add_text("Hardware Communications", color=(0, 200, 255))
+        dpg.add_input_text(label="ESP32 IP", default_value=cfg.esp32_ip, callback=lambda s, d: setattr(cfg, 'esp32_ip', d))
+        dpg.add_input_text(label="Serial Port", default_value=cfg.port, callback=lambda s, d: setattr(cfg, 'port', d))
+
+    dpg.setup_dearpygui()
+    dpg.show_viewport()
+
+
+def resolve_active_command(gamepad_cmd: dict | None) -> dict:
+    """Selects motion vector based on current active source in GUI."""
+    source = input_config["source"]
+    now = time.time()
+
+    if source == "GUI Sliders":
+        vx = input_config["manual_vx"]
+        vy = input_config["manual_vy"]
+        omega = input_config["manual_omega"]
+        joy_count = gamepad_cmd.get("joysticks_count", 0) if gamepad_cmd else 0
+
+    elif source == "Random Walk":
+        # Generate new random direction vector periodically
+        if now - input_config["last_random_time"] > input_config["random_interval"]:
+            input_config["random_vx"] = round(random.uniform(-1.0, 1.0), 2)
+            input_config["random_vy"] = round(random.uniform(-1.0, 1.0), 2)
+            input_config["random_omega"] = round(random.uniform(-1.0, 1.0), 2)
+            input_config["last_random_time"] = now
+
+        vx = input_config["random_vx"]
+        vy = input_config["random_vy"]
+        omega = input_config["random_omega"]
+        joy_count = gamepad_cmd.get("joysticks_count", 0) if gamepad_cmd else 0
+
+    else:  # "Gamepad"
+        if gamepad_cmd:
+            vx, vy, omega = gamepad_cmd.get("dirmov", [0.0, 0.0, 0.0])
+            joy_count = gamepad_cmd.get("joysticks_count", 0)
+        else:
+            vx, vy, omega, joy_count = 0.0, 0.0, 0.0, 0
+
+    state = "walking" if (vx != 0.0 or vy != 0.0 or omega != 0.0) else "idle"
+
+    # Update GUI Labels
+    if dpg.is_dearpygui_running():
+        dpg.set_value("txt_joy_count", f"Joysticks Connected: {joy_count}")
+        dpg.set_value("txt_motion_state", f"Motion State: {state.upper()} ({source})")
+        dpg.set_value("txt_vx", f"Active Vx   : {vx:>6.2f}")
+        dpg.set_value("txt_vy", f"Active Vy   : {vy:>6.2f}")
+        dpg.set_value("txt_omega", f"Active Omega: {omega:>6.2f}")
+
+    return {
+        "state": state,
+        "dirmov": [vx, vy, omega]
+    }
+
+
+def render_gui_frame():
+    if dpg.is_dearpygui_running():
+        dpg.render_dearpygui_frame()
+
+
+def stop_gui():
+    dpg.destroy_context()

inputs/PygameController.py

@@ -1,5 +1,5 @@
 """
-inputs/PygameController.py - Pygame Joystick Input Provider with Corrected Axes
+inputs/PygameController.py - Headless Pygame Joystick Provider
 """
 import math
 from multiprocessing import Queue
@@ -13,10 +13,6 @@ class PygameController(InputProvider):
         pygame.init()
         pygame.joystick.init()
         
-        self.screen = pygame.display.set_mode((400, 300))
-        pygame.display.set_caption("JackBot Controller Input")
-        self.font = pygame.font.Font(None, 24)
-        
         self.deadzone = deadzone
         self.joysticks = {}
         self.clock = pygame.time.Clock()
@@ -33,43 +29,37 @@ class PygameController(InputProvider):
         vx, vy, omega = 0.0, 0.0, 0.0
         emote = ""
 
-        # Process Pygame events
+        # Pump Pygame events in headless mode
         for event in pygame.event.get():
-            if event.type == pygame.QUIT:
+            if event.type == pygame.JOYDEVICEADDED:
-                pass
-            elif event.type == pygame.JOYDEVICEADDED:
                 joy = pygame.joystick.Joystick(event.device_index)
                 self.joysticks[joy.get_instance_id()] = joy
-                print(f"Joystick {joy.get_instance_id()} connected: {joy.get_name()}")
+                print(f"[Controller] Joystick {joy.get_instance_id()} connected: {joy.get_name()}")
             elif event.type == pygame.JOYDEVICEREMOVED:
                 if event.instance_id in self.joysticks:
-                    print(f"Joystick {event.instance_id} disconnected")
+                    print(f"[Controller] Joystick {event.instance_id} disconnected")
                     del self.joysticks[event.instance_id]
             elif event.type == pygame.JOYBUTTONDOWN:
                 if event.button == 3:
                     emote = "wave"
 
-        # Read active controller stick states
+        # Read stick values if joystick is connected
         if self.joysticks:
             for joystick in self.joysticks.values():
-                # Axis 1 = Left Stick Up/Down (Negated so Forward = +1.0)
-                # Axis 0 = Left Stick Left/Right (Right = +1.0)
                 raw_vx = -joystick.get_axis(1)  # Forward (+) / Backward (-)
                 raw_vy = joystick.get_axis(0)   # Right (+) / Left (-)
                 
-                # Check Axis 3 first for Right Stick X, fallback to Axis 2
                 num_axes = joystick.get_numaxes()
                 raw_omega = joystick.get_axis(3) if num_axes > 3 else joystick.get_axis(2)
 
                 norm_vx, norm_vy = self._normalize_input(raw_vx, raw_vy)
 
-                # Apply deadzones
                 if abs(raw_omega) >= self.deadzone:
                     omega = raw_omega
                 if abs(norm_vx) >= self.deadzone or abs(norm_vy) >= self.deadzone:
                     vx, vy = norm_vx, norm_vy
 
-                # Axis snapping for clean cardinal walking
+                # Cardinal snapping
                 if 0.8 < vx and -0.2 < vy < 0.2:
                     vx, vy = 1.0, 0.0
                 elif vx < -0.8 and -0.2 < vy < 0.2:
@@ -79,7 +69,6 @@ class PygameController(InputProvider):
                 elif -0.2 < vx < 0.2 and vy < -0.8:
                     vx, vy = 0.0, -1.0
 
-        # Determine robot state
         state = "walking" if (vx != 0.0 or vy != 0.0 or omega != 0.0) else "idle"
 
         self.current_command = CommandFrame(
@@ -87,24 +76,6 @@ class PygameController(InputProvider):
             vector_dirmov=[vx, vy, omega],
             emote=emote
         )
-
-        # Render visual display
-        self.screen.fill((30, 30, 30))
-        
-        axis_count = list(self.joysticks.values())[0].get_numaxes() if self.joysticks else 0
-        
-        lines = [
-            f"Connected Joysticks: {len(self.joysticks)} (Axes: {axis_count})",
-            f"Robot State: {self.current_command.state.upper()}",
-            f"Vx (Fwd/Bwd): {vx:>6.2f}",
-            f"Vy (Strafe) : {vy:>6.2f}",
-            f"Omega (Yaw) : {omega:>6.2f}",
-        ]
-        for i, line in enumerate(lines):
-            txt = self.font.render(line, True, (255, 255, 255))
-            self.screen.blit(txt, (20, 20 + i * 30))
-        
-        pygame.display.flip()
         return self.current_command
 
     def get_command(self) -> CommandFrame:
@@ -122,7 +93,8 @@ def controller_loop(control_queue: Queue):
             control_queue.put({
                 "state": cmd.state,
                 "dirmov": cmd.vector_dirmov,
-                "emote": cmd.emote
+                "emote": cmd.emote,
+                "joysticks_count": len(provider.joysticks)
             })
             provider.clock.tick(30)
     finally:

kinematics.py

@@ -8,7 +8,7 @@ import time
 import warnings
 warnings.filterwarnings("ignore", category=UserWarning, module="ikpy")
 
-import config as cfg
+from config import cfg
 import DataTypes as dt
 
 leg_chains = {

main.py

@@ -1,67 +1,85 @@
 """
-main.py - Entry point using object-oriented Robot and Input abstractions
+main.py - Entry point handling dynamic input source selection
 """
-from threading import Thread
+from multiprocessing import Queue, Process
-from multiprocessing import Queue
 import time
 
-import config as cfg
+from config import cfg
-from Robot import Robot, HardwareBackend, PyBulletBackend
+from Robot import Robot
-from simulation import Simulation
+from gui.MainWindow import (
-from EspCommunication import ESP32Communication
+    create_parameter_gui, 
-from ArduinoCommunication import ArduinoCommunication
+    render_gui_frame, 
+    stop_gui, 
+    resolve_active_command
+)
 from inputs.PygameController import controller_loop
 
-
+robot_instance: Robot | None = None
-def create_robot_instance() -> Robot:
+is_robot_active = False
-    if cfg.sim:
-        sim_instance = Simulation()
-        backend = PyBulletBackend(sim_instance)
-    else:
-        comm = ArduinoCommunication() if cfg.arduinoConnection else ESP32Communication()
-        comm.start()
-        backend = HardwareBackend(comm)
-
-    return Robot(backend=backend)
 
 
-def robot_control_loop(robot: Robot, control_queue: Queue):
+def start_robot():
-    robot.reset_to_init()
+    global robot_instance, is_robot_active
-    time.sleep(1)
+    print(f"[Main] Starting Robot with Backend: {cfg.backend.value.upper()}")
+    robot_instance = Robot(backend_type=cfg.backend)
+    robot_instance.reset_to_init()
+    is_robot_active = True
+
+
+def stop_robot():
+    global robot_instance, is_robot_active
+    print("[Main] Stopping Robot...")
+    is_robot_active = False
+    if robot_instance is not None:
+        robot_instance.cleanup()
+        robot_instance = None
+
+
+def main_event_loop(control_queue: Queue):
+    global robot_instance, is_robot_active
+
+    create_parameter_gui(
+        on_start_callback=start_robot,
+        on_stop_callback=stop_robot
+    )
 
-    tick = 0.05
     next_time = time.time()
 
+    try:
         while True:
-        next_time += tick
+            next_time += cfg.tick_duration
 
-        # Drain the queue to get the latest command frame
+            # Drain latest gamepad frame if available
-        latest_cmd = None
+            gamepad_cmd = None
             while not control_queue.empty():
-            latest_cmd = control_queue.get_nowait()
+                gamepad_cmd = control_queue.get_nowait()
 
-        if latest_cmd is not None:
+            # Resolve motion vector based on current active dropdown mode
-            robot.robot_state = latest_cmd["state"]
+            active_cmd = resolve_active_command(gamepad_cmd)
-            robot.vector_dirmov = latest_cmd["dirmov"]
 
-        # Run state machine tick
+            # Pass inputs to robot and tick state machine
-        robot.tick()
+            if is_robot_active and robot_instance is not None:
+                robot_instance.robot_state = active_cmd["state"]
+                robot_instance.vector_dirmov = active_cmd["dirmov"]
+                robot_instance.tick()
+
+            render_gui_frame()
 
             sleep_time = next_time - time.time()
             if sleep_time > 0:
                 time.sleep(sleep_time)
 
+    finally:
+        stop_gui()
+
 
 if __name__ == "__main__":
-    controller_queue = Queue()
+    control_queue = Queue()
     
-    my_robot = create_robot_instance()
+    controller_process = Process(target=controller_loop, args=(control_queue,))
+    controller_process.start()
 
-    # Start Pygame input thread
+    try:
-    input_thread = Thread(target=controller_loop, args=(controller_queue,), daemon=True)
+        main_event_loop(control_queue)
-    robot_thread = Thread(target=robot_control_loop, args=(my_robot, controller_queue), daemon=True)
+    finally:
-
+        controller_process.terminate()
-    input_thread.start()
-    robot_thread.start()
-
-    robot_thread.join()

simulation.py

@@ -5,21 +5,31 @@ import time
 import math
 import numpy as np
 import pybullet as p
+import pybullet_data  # Added missing import
 
-import config as cfg
+from config import cfg
 import DataTypes as dt
 
 
 class Simulation:
-    def __init__(self):
+    def __init__(self, urdf_path: str = cfg.urdf_path):
-        self.physics_client = p.connect(p.GUI)
+        self.urdf_path = urdf_path
-        self.robot = p.loadURDF(cfg.urdf_path, useFixedBase=True)
         
-        self.revolute_joints = [
+        # Connect to PyBullet GUI
-            i
+        self.physicsClient = p.connect(p.GUI)
-            for i in range(p.getNumJoints(self.robot))
+        p.setAdditionalSearchPath(pybullet_data.getDataPath())
-            if p.getJointInfo(self.robot, i)[2] == p.JOINT_REVOLUTE
+        p.setGravity(0, 0, -9.81)
-        ]
+        
+        # Load plane and robot URDF
+        self.planeId = p.loadURDF("plane.urdf")
+        self.robot = p.loadURDF(self.urdf_path, [0, 0, 0.2])
+
+        # Discover revolute joint indices dynamically
+        self.revolute_joints = []
+        for j in range(p.getNumJoints(self.robot)):
+            joint_info = p.getJointInfo(self.robot, j)
+            if joint_info[2] == p.JOINT_REVOLUTE:
+                self.revolute_joints.append(j)
 
         self.set_all_joints_to_90()
         p.resetDebugVisualizerCamera(
@@ -30,10 +40,7 @@ class Simulation:
         )
 
     def set_all_joints_to_90(self):
-        for joint_index in range(p.getNumJoints(self.robot)):
+        for joint_index in self.revolute_joints:
-            joint_info = p.getJointInfo(self.robot, joint_index)
-            joint_type = joint_info[2]
-            if joint_type == p.JOINT_REVOLUTE:
             p.resetJointState(self.robot, joint_index, math.radians(90))
 
     def updatePos(self, current_rad: dt.RadArray):
@@ -51,7 +58,12 @@ class Simulation:
         p.stepSimulation()
 
     def disconnect(self):
-        p.disconnect(self.physics_client)
+        if p.isConnected(self.physicsClient):
+            p.disconnect(self.physicsClient)
+
+    def close(self):
+        """Cleanup wrapper for Robot backend compatibility."""
+        self.disconnect()
 
 
 if __name__ == "__main__":
@@ -62,7 +74,7 @@ if __name__ == "__main__":
     backend = PyBulletBackend(sim_instance)
 
     # 2. Instantiate Robot with simulation backend
-    robot = Robot(backend=backend)
+    robot = Robot(backend_type=backend)
     robot.reset_to_init()
 
     # 3. Command forward movement [vx, vy, omega]
@@ -71,7 +83,7 @@ if __name__ == "__main__":
     # 4. Main test execution loop
     try:
         while True:
-            # Executes state machine logic (Idle -> Walking -> Target Step -> Physics Step)
+            # Executes state machine logic
             robot.tick()
             time.sleep(1.0 / 60.0)
     except KeyboardInterrupt:

states/WalkingState.py

@@ -6,7 +6,7 @@ from typing import TYPE_CHECKING, Optional
 import math
 import numpy as np
 
-import config as cfg
+from config import cfg
 import DataTypes as dt
 from states.State import State
 
@@ -24,8 +24,8 @@ class WalkingState(State):
 
     def __init__(
         self,
-        duration: float = cfg.standard_duration,
+        duration: float = cfg.step_duration,
-        tickpersec: float = cfg.standard_tickpersec,
+        tickpersec: float = cfg.tick_rate_hz,
     ):
         self.duration = duration
         self.tickpersec = tickpersec
@@ -151,8 +151,8 @@ class WalkingFourState(State):
 
     def __init__(
         self,
-        duration: float = cfg.standard_duration,
+        duration: float = cfg.step_duration,
-        tickpersec: float = cfg.standard_tickpersec,
+        tickpersec: float = cfg.tick_rate_hz,
     ):
         self.duration = duration
         self.tickpersec = tickpersec

states/WaveState.py

@@ -6,7 +6,7 @@ from typing import TYPE_CHECKING, Optional
 import numpy as np
 import math
 
-import config as cfg
+from config import cfg
 import DataTypes as dt
 from states.State import State
 if TYPE_CHECKING: