JackM323/JackBot
1
0
Fork 0
Code Issues 3 Pull Requests Actions Packages Projects 2 Releases Wiki Activity

Added Gui (unfinished)

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

small fixes (import, names...)
This commit is contained in:
JackM323
2026-07-30 22:49:53 +02:00
parent b537677277
commit 9c31de3c38
13 changed files with 383 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1 -1
View File
@@ -1,7 +1,7 @@
# Ignore dependency folders
node_modules/
.venv/
.vs/
.vscode/
__pycache__/
ml/checkpoints/
ml/tensorboard/
ArduinoCommunication.py
+3 -3
View File
@@ -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
+1 -1
View File
@@ -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
-39
View File
@@ -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
+42 -52
View File
@@ -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
+41 -25
View File
@@ -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()
gui/MainWindow.py
+184
View File
@@ -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
+9 -37
View File
@@ -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:
kinematics.py
+1 -1
View File
@@ -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
+65 -47
View File
@@ -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()
try:
main_event_loop(control_queue)
finally:
controller_process.terminate()
simulation.py
+29 -17
View File
@@ -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
self.revolute_joints = [
i
for i in range(p.getNumJoints(self.robot))
if p.getJointInfo(self.robot, i)[2] == p.JOINT_REVOLUTE
]
# 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])
# 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:
states/WalkingState.py
+5 -5
View File
@@ -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
states/WaveState.py
+1 -1
View File
@@ -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: