sladro
0119d9365f
功能更新： - 实现KDL自动链检测，无需手动配置链接名称 - 创建ArmController统一控制接口，集成KDL与PyBullet - 更新MainWindow使用ArmController替换独立的RobotLoader - 修复KDL固定关节类型语法错误 已知问题： - KDL逆运动学在Windows上崩溃，需要进一步调试 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
448 lines
18 KiB
Python
448 lines
18 KiB
Python
import tkinter as tk
|
|
from tkinter import ttk, messagebox, scrolledtext
|
|
import pybullet as p
|
|
import pybullet_data
|
|
import threading
|
|
import time
|
|
from typing import Dict, Any
|
|
import sys
|
|
import os
|
|
|
|
# Add parent directory to path for imports
|
|
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))))
|
|
|
|
from src.config_loader import ConfigLoader
|
|
from src.robot.arm_controller import create_arm_controller
|
|
from src.simulation.environment import Environment
|
|
from src.gui.config_window import ConfigWindow
|
|
|
|
|
|
class MainWindow:
|
|
def __init__(self):
|
|
self.root = tk.Tk()
|
|
self.root.title("Robotic Arm Simulation - Feasibility Test")
|
|
|
|
# Get window dimensions from config or use defaults
|
|
self._setup_window_geometry()
|
|
|
|
# Simulation components
|
|
self.physics_client = None
|
|
self.config_loader = None
|
|
self.arm_controller = None
|
|
self.environment = None
|
|
self.config_window = None
|
|
|
|
# Simulation state
|
|
self.simulation_running = False
|
|
self.simulation_thread = None
|
|
|
|
# Create GUI
|
|
self._create_control_panel()
|
|
self._create_log_panel()
|
|
self._create_bottom_panel()
|
|
|
|
# Initialize simulation
|
|
self.initialize_simulation()
|
|
|
|
def _setup_window_geometry(self):
|
|
"""Setup window geometry from config or defaults"""
|
|
try:
|
|
# Try to load from config if exists
|
|
config = ConfigLoader().get_full_config()
|
|
window_size = config.get('gui', {}).get('window_size', "1200x800")
|
|
self.root.geometry(window_size)
|
|
except Exception as e:
|
|
# Use config default if loading fails
|
|
default_config = {"gui": {"window_size": "1200x800"}}
|
|
self.root.geometry(default_config['gui']['window_size'])
|
|
|
|
def _create_control_panel(self):
|
|
"""Create control panel on the left side"""
|
|
# Create main frame
|
|
self.main_frame = tk.Frame(self.root)
|
|
self.main_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
|
|
|
|
# Left panel - Control panel
|
|
left_panel = tk.Frame(self.main_frame, width=300)
|
|
left_panel.pack(side=tk.LEFT, fill=tk.Y, padx=(0, 10))
|
|
|
|
# Title
|
|
title_label = tk.Label(left_panel, text="Control Panel",
|
|
font=("Arial", 14, "bold"))
|
|
title_label.pack(pady=(0, 10))
|
|
|
|
# Configuration section
|
|
config_frame = tk.LabelFrame(left_panel, text="Configuration", padx=10, pady=10)
|
|
config_frame.pack(fill=tk.X, pady=(0, 10))
|
|
|
|
self.config_btn = tk.Button(config_frame, text="Open Config Manager",
|
|
command=self.open_config_window,
|
|
bg="#2196F3", fg="white", padx=10, pady=5)
|
|
self.config_btn.pack(fill=tk.X)
|
|
|
|
self.reload_btn = tk.Button(config_frame, text="Reload Config",
|
|
command=self.reload_configuration,
|
|
padx=10, pady=5)
|
|
self.reload_btn.pack(fill=tk.X, pady=(5, 0))
|
|
|
|
# Simulation control section
|
|
control_frame = tk.LabelFrame(left_panel, text="Simulation Control", padx=10, pady=10)
|
|
control_frame.pack(fill=tk.X, pady=(0, 10))
|
|
|
|
self.start_btn = tk.Button(control_frame, text="Start Simulation",
|
|
command=self.start_simulation,
|
|
bg="#4CAF50", fg="white", padx=10, pady=5)
|
|
self.start_btn.pack(fill=tk.X)
|
|
|
|
self.pause_btn = tk.Button(control_frame, text="Pause Simulation",
|
|
command=self.pause_simulation,
|
|
state=tk.DISABLED, padx=10, pady=5)
|
|
self.pause_btn.pack(fill=tk.X, pady=(5, 0))
|
|
|
|
self.reset_btn = tk.Button(control_frame, text="Reset Environment",
|
|
command=self.reset_environment,
|
|
bg="#FF9800", fg="white", padx=10, pady=5)
|
|
self.reset_btn.pack(fill=tk.X, pady=(5, 0))
|
|
|
|
# Reachability test section
|
|
test_frame = tk.LabelFrame(left_panel, text="Reachability Test", padx=10, pady=10)
|
|
test_frame.pack(fill=tk.X, pady=(0, 10))
|
|
|
|
self.test_btn = tk.Button(test_frame, text="Test Reachability",
|
|
command=self.test_reachability,
|
|
bg="#9C27B0", fg="white", padx=10, pady=5)
|
|
self.test_btn.pack(fill=tk.X)
|
|
|
|
tk.Label(test_frame, text="Check if A and B points are reachable",
|
|
font=("Arial", 9)).pack(pady=(5, 0))
|
|
|
|
# Status section
|
|
status_frame = tk.LabelFrame(left_panel, text="Status", padx=10, pady=10)
|
|
status_frame.pack(fill=tk.X, pady=(0, 10))
|
|
|
|
# Robot status
|
|
tk.Label(status_frame, text="Robot:", font=("Arial", 9, "bold")).grid(row=0, column=0, sticky="w")
|
|
self.robot_status = tk.Label(status_frame, text="Not Loaded", fg="red")
|
|
self.robot_status.grid(row=0, column=1, sticky="w", padx=(5, 0))
|
|
|
|
# Environment status
|
|
tk.Label(status_frame, text="Environment:", font=("Arial", 9, "bold")).grid(row=1, column=0, sticky="w")
|
|
self.env_status = tk.Label(status_frame, text="Not Loaded", fg="red")
|
|
self.env_status.grid(row=1, column=1, sticky="w", padx=(5, 0))
|
|
|
|
# Simulation status
|
|
tk.Label(status_frame, text="Simulation:", font=("Arial", 9, "bold")).grid(row=2, column=0, sticky="w")
|
|
self.sim_status = tk.Label(status_frame, text="Stopped", fg="orange")
|
|
self.sim_status.grid(row=2, column=1, sticky="w", padx=(5, 0))
|
|
|
|
|
|
def _create_log_panel(self):
|
|
"""Create log panel with copyable text"""
|
|
from tkinter import scrolledtext
|
|
|
|
# Right panel - Log display
|
|
right_panel = tk.Frame(self.main_frame)
|
|
right_panel.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
|
|
|
|
# Log display frame
|
|
log_label = tk.Label(right_panel, text="System Log", font=("Arial", 12, "bold"))
|
|
log_label.pack(pady=(0, 5))
|
|
|
|
# Scrollable text widget for logs (can select and copy)
|
|
self.log_text = scrolledtext.ScrolledText(right_panel, height=25, wrap=tk.WORD)
|
|
self.log_text.pack(fill=tk.BOTH, expand=True, padx=5, pady=5)
|
|
|
|
# Make it read-only initially but still selectable
|
|
self.log_text.config(state=tk.DISABLED)
|
|
|
|
def _create_bottom_panel(self):
|
|
"""Create bottom panel with viewer info and exit button"""
|
|
bottom_frame = tk.Frame(self.root)
|
|
bottom_frame.pack(fill=tk.X, padx=10, pady=(0, 10))
|
|
|
|
tk.Label(bottom_frame, text="PyBullet Viewer:", font=("Arial", 9, "bold")).pack(side=tk.LEFT, padx=(0, 5))
|
|
tk.Label(bottom_frame, text="Use external PyBullet GUI window for 3D visualization",
|
|
fg="gray").pack(side=tk.LEFT)
|
|
|
|
# Exit button
|
|
exit_btn = tk.Button(bottom_frame, text="Exit", command=self.on_closing,
|
|
bg="#f44336", fg="white", padx=20)
|
|
exit_btn.pack(side=tk.RIGHT)
|
|
|
|
def initialize_simulation(self):
|
|
"""Initialize PyBullet simulation"""
|
|
try:
|
|
# Connect to PyBullet in GUI mode
|
|
self.physics_client = p.connect(p.GUI)
|
|
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
|
|
|
# Configure visualization
|
|
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
|
|
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)
|
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
|
|
|
# Set camera from config or defaults
|
|
self._reset_camera_view()
|
|
|
|
# Load configuration
|
|
self.config_loader = ConfigLoader()
|
|
|
|
# Setup environment
|
|
self.environment = Environment(self.config_loader, self.physics_client)
|
|
self.environment.setup_environment()
|
|
self.env_status.config(text="Loaded", fg="green")
|
|
|
|
# Initialize arm controller
|
|
self.arm_controller = create_arm_controller(self.config_loader, self.physics_client)
|
|
self.robot_status.config(text="Loaded", fg="green")
|
|
|
|
self.update_status("System ready")
|
|
|
|
except Exception as e:
|
|
self.update_status(f"Initialization failed: {e}")
|
|
messagebox.showerror("Initialization Error", f"Failed to initialize simulation:\n{e}")
|
|
|
|
def open_config_window(self):
|
|
"""Open configuration management window"""
|
|
if not self.config_window:
|
|
self.config_window = ConfigWindow(
|
|
self.root,
|
|
on_apply_callback=self.on_config_applied
|
|
)
|
|
self.config_window.show()
|
|
|
|
def on_config_applied(self, new_config: Dict[str, Any]):
|
|
"""Handle configuration changes from config window"""
|
|
self.update_status("Applying configuration...")
|
|
|
|
try:
|
|
# Save current robot state if exists
|
|
robot_state = None
|
|
if self.arm_controller and self.arm_controller.is_initialized:
|
|
robot_state = self.arm_controller.get_current_joint_states()
|
|
|
|
# Clear current environment
|
|
if self.environment:
|
|
self.environment.cleanup()
|
|
# Note: Robot cleanup is handled by ArmController internally
|
|
|
|
# Create new ConfigLoader with updated configuration
|
|
self.config_loader = ConfigLoader()
|
|
# Update the config loader's internal config directly
|
|
self.config_loader._config = new_config
|
|
|
|
# Recreate environment
|
|
self.environment = Environment(self.config_loader, self.physics_client)
|
|
self.environment.setup_environment()
|
|
|
|
# Recreate arm controller
|
|
self.arm_controller = create_arm_controller(self.config_loader, self.physics_client)
|
|
|
|
# Restore robot state if available
|
|
if robot_state:
|
|
try:
|
|
positions = [state['position'] for state in robot_state.values()]
|
|
self.arm_controller.move_to_joint_positions(positions)
|
|
except Exception:
|
|
# State restoration failed, continue with defaults
|
|
pass
|
|
|
|
self.update_status("Configuration applied")
|
|
|
|
except Exception as e:
|
|
self.update_status(f"Configuration error: {e}")
|
|
messagebox.showerror("Configuration Error", f"Failed to apply configuration:\n{e}")
|
|
|
|
def reload_configuration(self):
|
|
"""Reload configuration from file"""
|
|
try:
|
|
self.config_loader = ConfigLoader()
|
|
self.on_config_applied(self.config_loader.get_full_config())
|
|
self.update_status("Configuration reloaded")
|
|
except Exception as e:
|
|
self.update_status(f"Reload failed: {e}")
|
|
|
|
def start_simulation(self):
|
|
"""Start simulation loop"""
|
|
if not self.simulation_running:
|
|
self.simulation_running = True
|
|
self.sim_status.config(text="Running", fg="green")
|
|
self.start_btn.config(state=tk.DISABLED)
|
|
self.pause_btn.config(state=tk.NORMAL)
|
|
|
|
# Start simulation thread
|
|
self.simulation_thread = threading.Thread(target=self.simulation_loop, daemon=True)
|
|
self.simulation_thread.start()
|
|
|
|
self.update_status("Simulation running")
|
|
|
|
def pause_simulation(self):
|
|
"""Pause simulation loop"""
|
|
if self.simulation_running:
|
|
self.simulation_running = False
|
|
self.sim_status.config(text="Paused", fg="orange")
|
|
self.start_btn.config(state=tk.NORMAL)
|
|
self.pause_btn.config(state=tk.DISABLED)
|
|
self.update_status("Simulation paused")
|
|
|
|
def simulation_loop(self):
|
|
"""Main simulation loop (runs in separate thread)"""
|
|
while self.simulation_running:
|
|
try:
|
|
p.stepSimulation(physicsClientId=self.physics_client)
|
|
# Get timestep from config
|
|
timestep = self.config_loader.get_full_config().get('simulation', {}).get('timestep', 0.01)
|
|
time.sleep(timestep)
|
|
except Exception as e:
|
|
self.update_status(f"Simulation error: {e}")
|
|
self.simulation_running = False
|
|
break
|
|
|
|
def reset_environment(self):
|
|
"""Reset environment to initial state"""
|
|
try:
|
|
# Reset robot to home position
|
|
if self.arm_controller and self.arm_controller.is_initialized:
|
|
self.arm_controller.reset_to_home_position()
|
|
|
|
# Reset environment objects
|
|
if self.environment:
|
|
self.environment.reset_environment()
|
|
|
|
# Reset camera
|
|
self._reset_camera_view()
|
|
|
|
self.update_status("Environment reset")
|
|
|
|
except Exception as e:
|
|
self.update_status(f"Reset failed: {e}")
|
|
|
|
def test_reachability(self):
|
|
"""Test reachability of task points"""
|
|
self.update_status("Testing reachability...")
|
|
|
|
try:
|
|
# Get task points
|
|
task_points = self.config_loader.get_task_points()
|
|
point_a = task_points['point_A']['position']
|
|
point_b = task_points['point_B']['position']
|
|
|
|
# Perform reachability check - this will expose the KDL problem
|
|
self._check_reachability(point_a, point_b)
|
|
|
|
except Exception as e:
|
|
import traceback
|
|
error_msg = f"Reachability test failed: {str(e)}"
|
|
self.update_status(error_msg)
|
|
print(f"ERROR: {error_msg}")
|
|
print("Full traceback:")
|
|
traceback.print_exc()
|
|
|
|
def _check_reachability(self, point_a, point_b):
|
|
"""Check if points are reachable by the robot using KDL kinematics"""
|
|
try:
|
|
# Use ArmController's precise reachability checking with KDL
|
|
# This will call KDL inverse kinematics and expose any problems
|
|
a_reachable = self.arm_controller.check_workspace_reachability(point_a)
|
|
b_reachable = self.arm_controller.check_workspace_reachability(point_b)
|
|
|
|
# Get detailed results
|
|
results = []
|
|
if a_reachable:
|
|
results.append("Point A: Reachable")
|
|
else:
|
|
results.append("Point A: Not reachable")
|
|
|
|
if b_reachable:
|
|
results.append("Point B: Reachable")
|
|
else:
|
|
results.append("Point B: Not reachable")
|
|
|
|
# Update status based on results
|
|
if a_reachable and b_reachable:
|
|
self.update_status("Both points are reachable")
|
|
else:
|
|
self.update_status(f"Reachability: {', '.join(results)}")
|
|
|
|
except Exception as e:
|
|
import traceback
|
|
error_msg = f"Reachability check failed: {str(e)}"
|
|
self.update_status(error_msg)
|
|
print(f"ERROR in _check_reachability: {error_msg}")
|
|
print("Full traceback:")
|
|
traceback.print_exc()
|
|
|
|
def _calculate_distance(self, point):
|
|
"""Calculate Euclidean distance from origin"""
|
|
return (point[0]**2 + point[1]**2 + point[2]**2) ** 0.5
|
|
|
|
def _reset_camera_view(self):
|
|
"""Reset camera to default view using config values"""
|
|
try:
|
|
# Get camera settings from config
|
|
config = self.config_loader.get_full_config() if self.config_loader else {}
|
|
camera_config = config.get('camera', {})
|
|
|
|
# Use config values with explicit defaults from config structure
|
|
default_camera = {'distance': 4.0, 'yaw': 45, 'pitch': -30, 'target': [0, 0, 0]}
|
|
distance = camera_config.get('distance', default_camera['distance'])
|
|
yaw = camera_config.get('yaw', default_camera['yaw'])
|
|
pitch = camera_config.get('pitch', default_camera['pitch'])
|
|
target = camera_config.get('target', default_camera['target'])
|
|
|
|
p.resetDebugVisualizerCamera(
|
|
cameraDistance=distance,
|
|
cameraYaw=yaw,
|
|
cameraPitch=pitch,
|
|
cameraTargetPosition=target,
|
|
physicsClientId=self.physics_client
|
|
)
|
|
except Exception as e:
|
|
# Re-raise exception for proper error handling
|
|
raise Exception(f"Failed to reset camera view: {e}")
|
|
|
|
def update_status(self, message: str):
|
|
"""Update status display with copyable text"""
|
|
import time
|
|
if hasattr(self, 'log_text'):
|
|
# Enable text widget to add content
|
|
self.log_text.config(state=tk.NORMAL)
|
|
# Add timestamp and message
|
|
timestamp = time.strftime("%H:%M:%S")
|
|
self.log_text.insert(tk.END, f"[{timestamp}] {message}\n")
|
|
# Auto-scroll to bottom
|
|
self.log_text.see(tk.END)
|
|
# Make read-only again but still selectable
|
|
self.log_text.config(state=tk.DISABLED)
|
|
|
|
def on_closing(self):
|
|
"""Handle window closing"""
|
|
if messagebox.askokcancel("Quit", "Do you want to quit the simulation?"):
|
|
try:
|
|
# Stop simulation
|
|
self.simulation_running = False
|
|
|
|
# Cleanup
|
|
if self.environment:
|
|
self.environment.cleanup()
|
|
|
|
# Disconnect PyBullet
|
|
if self.physics_client is not None:
|
|
p.disconnect(self.physics_client)
|
|
|
|
except Exception as e:
|
|
# Log cleanup failure but still close
|
|
print(f"Cleanup error: {e}")
|
|
|
|
self.root.destroy()
|
|
|
|
def run(self):
|
|
"""Start the main window"""
|
|
self.root.protocol("WM_DELETE_WINDOW", self.on_closing)
|
|
self.root.mainloop()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# Test the main window directly
|
|
app = MainWindow()
|
|
app.run() |