diff --git a/.gitignore b/.gitignore index 3eabf78..3098c3f 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,7 @@ # Ignore dependency folders node_modules/ .venv/ -.vs/ +.vscode/ __pycache__/ ml/checkpoints/ ml/tensorboard/ diff --git a/ArduinoCommunication.py b/ArduinoCommunication.py index f2a709d..ecc8609 100644 --- a/ArduinoCommunication.py +++ b/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 diff --git a/EspCommunication.py b/EspCommunication.py index e4f4770..60cbafd 100644 --- a/EspCommunication.py +++ b/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 # ============================================================ diff --git a/GlobalVariables.py b/GlobalVariables.py deleted file mode 100644 index d7a023e..0000000 --- a/GlobalVariables.py +++ /dev/null @@ -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 diff --git a/Robot.py b/Robot.py index 9c204a0..f00ce6c 100644 --- a/Robot.py +++ b/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() \ No newline at end of file + self.step_sim() + + def cleanup(self) -> None: + if self.backend and hasattr(self.backend, 'cleanup'): + self.backend.cleanup() \ No newline at end of file diff --git a/config.py b/config.py index ea46a56..9dd01dc 100644 --- a/config.py +++ b/config.py @@ -1,31 +1,47 @@ -# Connection -port = "COM4" -baudrate = 38400 +""" +config.py - Central Configuration & Parameters for JackBot +""" +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) -# column = servo position from torso(0) to feet(2) +@dataclass +class RobotConfig: + # ------------------------------------------------------------------------- + # 1. Execution & Backend Target + # ------------------------------------------------------------------------- + backend: BackendType = BackendType.SIMULATION + urdf_path: str = "JackBotUrdf.urdf" -# Dimensions in m -#robot_height = -0.1 -step_height = 0.03 -step_length = 0.04 -translation_gain = 1.0 -rotation_gain = 2.0 -# Leglength in m -# L1 = 0.068 -# L2 = 0.0602 -# L3 = 0.070 + # Hardware Connection Settings + port: str = "COM4" # Serial Port for Arduino + baudrate: int = 38400 # Baudrate for Serial + esp32_ip: str = "192.168.188.32" # Wi-Fi IP for ESP32 UDP communication + esp32_port: int = 3323 # UDP Target Port + comm_timeout: float = 2.0 # Connection timeout threshold [s] -# standard_tickpersec = # 20 Fluessige Bewegung -standard_tickpersec:float = 25 # [ticks/s] -standard_duration:float = 0.8 # [s] -standard_tickduration:float = 1 / standard_tickpersec + # ------------------------------------------------------------------------- + # 2. Gait & Motion Kinematics Parameters + # ------------------------------------------------------------------------- + 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() \ No newline at end of file diff --git a/gui/MainWindow.py b/gui/MainWindow.py new file mode 100644 index 0000000..7ce5d84 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/inputs/PygameController.py b/inputs/PygameController.py index df24081..205609c 100644 --- a/inputs/PygameController.py +++ b/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: - pass - elif event.type == pygame.JOYDEVICEADDED: + if 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: diff --git a/kinematics.py b/kinematics.py index 548a22e..5556749 100644 --- a/kinematics.py +++ b/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 = { diff --git a/main.py b/main.py index 447c373..27403dd 100644 --- a/main.py +++ b/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 +from multiprocessing import Queue, Process import time -import config as cfg -from Robot import Robot, HardwareBackend, PyBulletBackend -from simulation import Simulation -from EspCommunication import ESP32Communication -from ArduinoCommunication import ArduinoCommunication +from config import cfg +from Robot import Robot +from gui.MainWindow import ( + create_parameter_gui, + render_gui_frame, + stop_gui, + resolve_active_command +) from inputs.PygameController import controller_loop - -def create_robot_instance() -> Robot: - 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) +robot_instance: Robot | None = None +is_robot_active = False -def robot_control_loop(robot: Robot, control_queue: Queue): - robot.reset_to_init() - time.sleep(1) +def start_robot(): + global robot_instance, is_robot_active + 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() - while True: - next_time += tick + try: + while True: + next_time += cfg.tick_duration - # Drain the queue to get the latest command frame - latest_cmd = None - while not control_queue.empty(): - latest_cmd = control_queue.get_nowait() + # Drain latest gamepad frame if available + gamepad_cmd = None + while not control_queue.empty(): + gamepad_cmd = control_queue.get_nowait() - if latest_cmd is not None: - robot.robot_state = latest_cmd["state"] - robot.vector_dirmov = latest_cmd["dirmov"] + # Resolve motion vector based on current active dropdown mode + active_cmd = resolve_active_command(gamepad_cmd) - # Run state machine tick - robot.tick() + # Pass inputs to robot and tick state machine + 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() - sleep_time = next_time - time.time() - if sleep_time > 0: - time.sleep(sleep_time) + 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 - input_thread = Thread(target=controller_loop, args=(controller_queue,), daemon=True) - robot_thread = Thread(target=robot_control_loop, args=(my_robot, controller_queue), daemon=True) - - input_thread.start() - robot_thread.start() - - robot_thread.join() \ No newline at end of file + try: + main_event_loop(control_queue) + finally: + controller_process.terminate() \ No newline at end of file diff --git a/simulation.py b/simulation.py index 5f79bba..b8a7c2a 100644 --- a/simulation.py +++ b/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): - self.physics_client = p.connect(p.GUI) - self.robot = p.loadURDF(cfg.urdf_path, useFixedBase=True) + def __init__(self, urdf_path: str = cfg.urdf_path): + self.urdf_path = urdf_path + + # Connect to PyBullet GUI + self.physicsClient = p.connect(p.GUI) + p.setAdditionalSearchPath(pybullet_data.getDataPath()) + 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]) - self.revolute_joints = [ - i - for i in range(p.getNumJoints(self.robot)) - if p.getJointInfo(self.robot, i)[2] == p.JOINT_REVOLUTE - ] + # 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,11 +40,8 @@ class Simulation: ) def set_all_joints_to_90(self): - for joint_index in range(p.getNumJoints(self.robot)): - 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)) + for joint_index in self.revolute_joints: + p.resetJointState(self.robot, joint_index, math.radians(90)) def updatePos(self, current_rad: dt.RadArray): radflat = current_rad.data.flatten() @@ -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: diff --git a/states/WalkingState.py b/states/WalkingState.py index 6119c4a..98f4bc1 100644 --- a/states/WalkingState.py +++ b/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, - tickpersec: float = cfg.standard_tickpersec, + duration: float = cfg.step_duration, + 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, - tickpersec: float = cfg.standard_tickpersec, + duration: float = cfg.step_duration, + tickpersec: float = cfg.tick_rate_hz, ): self.duration = duration self.tickpersec = tickpersec diff --git a/states/WaveState.py b/states/WaveState.py index f366f6e..f280e1a 100644 --- a/states/WaveState.py +++ b/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: