diff --git a/config.json b/config.json
index dfa526f..712dd40 100644
--- a/config.json
+++ b/config.json
@@ -1,45 +1,74 @@
{
"robot": {
"model_path": "models/manual_robot.urdf",
- "base_position": [0.0, 0.0, 0.0],
- "base_orientation": [0.0, 0.0, 0.0]
+ "base_position": [
+ 0.0,
+ 1.6,
+ 0.0
+ ],
+ "base_orientation": [
+ 0.0,
+ 0.0,
+ 1.5
+ ]
},
-
"wall": {
- "position": [2.0, 0.0, 1.0],
+ "position": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
"dimensions": {
"width": 3.0,
"height": 2.5,
"thickness": 0.2
},
"material": {
- "color": [0.8, 0.8, 0.8, 1.0],
+ "color": [
+ 0.8,
+ 0.8,
+ 0.8,
+ 1.0
+ ],
"type": "concrete"
}
},
-
"hole": {
- "position_relative_to_wall": [0.0, 0.0, 0.0],
+ "position_relative_to_wall": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
"dimensions": {
"width": 0.6,
"height": 0.6
},
"shape": "rectangle"
},
-
"task_points": {
"point_A": {
- "position": [1.0, 0.5, 0.8],
+ "position": [
+ 1.0,
+ 0.5,
+ 0.8
+ ],
"description": "取物点，与机械臂同侧"
},
"point_B": {
- "position": [3.0, 0.5, 0.8],
+ "position": [
+ 3.0,
+ 0.5,
+ 0.8
+ ],
"description": "目标点，墙体另一侧"
}
},
-
"transport_object": {
- "initial_position": [1.0, 0.5, 0.5],
+ "initial_position": [
+ 1.0,
+ 0.5,
+ 0.5
+ ],
"dimensions": {
"width": 0.1,
"height": 0.1,
@@ -48,14 +77,35 @@
"mass": 0.5,
"shape": "cube",
"material": {
- "color": [1.0, 0.0, 0.0, 1.0],
+ "color": [
+ 1.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
"friction": 0.7
}
},
-
"simulation": {
"timestep": 0.01,
- "gravity": [0.0, 0.0, -9.81],
+ "gravity": [
+ 0.0,
+ 0.0,
+ -9.81
+ ],
"real_time": false
+ },
+ "camera": {
+ "distance": 4.0,
+ "yaw": 45,
+ "pitch": -30,
+ "target": [
+ 0,
+ 0,
+ 0
+ ]
+ },
+ "gui": {
+ "window_size": "1200x800"
}
}
\ No newline at end of file
diff --git a/main.py b/main.py
new file mode 100644
index 0000000..4b2744e
--- /dev/null
+++ b/main.py
@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+"""
+Robotic Arm Feasibility Test System
+
+Main entry point for the robotic arm simulation system.
+This system tests the feasibility of robotic arm operations in complex environments,
+specifically transporting objects through obstacles.
+"""
+
+import sys
+import json
+from pathlib import Path
+
+# Add src directory to Python path
+PROJECT_ROOT = Path(__file__).parent.absolute()
+sys.path.insert(0, str(PROJECT_ROOT / 'src'))
+
+from src.gui.main_window import MainWindow
+
+# Configuration constants
+CONFIG_FILE = "config.json"
+REQUIRED_MODULES = ['pybullet', 'tkinter', 'numpy']
+
+
+def check_dependencies():
+ """Check if all required dependencies are installed"""
+ missing = []
+
+ for module in REQUIRED_MODULES:
+ try:
+ __import__(module)
+ except ImportError:
+ missing.append(module)
+
+ if not missing:
+ return True
+
+ print(f"Missing dependencies: {', '.join(missing)}")
+ print(f"Install with: pip install {' '.join(missing)}")
+ return False
+
+
+def verify_environment():
+ """Verify that the environment is properly configured"""
+ # Check configuration file
+ config_path = PROJECT_ROOT / CONFIG_FILE
+ if not config_path.exists():
+ print(f"Error: {CONFIG_FILE} not found")
+ return False
+
+ # Check robot model file
+ try:
+ with open(config_path, 'r', encoding='utf-8') as f:
+ config = json.load(f)
+
+ model_path = config.get('robot', {}).get('model_path', '')
+ if model_path and not Path(model_path).exists():
+ print(f"Warning: Robot model not found: {model_path}")
+ # Continue anyway - let the simulation handle missing model
+ except (json.JSONDecodeError, IOError) as e:
+ print(f"Error reading configuration: {e}")
+ return False
+
+ return True
+
+
+def main():
+ """Main entry point"""
+ # Verify dependencies and environment
+ if not check_dependencies():
+ sys.exit(1)
+
+ if not verify_environment():
+ sys.exit(1)
+
+ # Start application
+ try:
+ app = MainWindow()
+ app.run()
+ except KeyboardInterrupt:
+ pass # Clean exit on Ctrl+C
+ except Exception as e:
+ print(f"Fatal error: {e}")
+ sys.exit(1)
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/models/link_0.stl b/models/link_0.stl
new file mode 100644
index 0000000..91f3cc0
Binary files /dev/null and b/models/link_0.stl differ
diff --git a/models/link_1.stl b/models/link_1.stl
new file mode 100644
index 0000000..de96389
Binary files /dev/null and b/models/link_1.stl differ
diff --git a/models/link_2.stl b/models/link_2.stl
new file mode 100644
index 0000000..f9451bd
Binary files /dev/null and b/models/link_2.stl differ
diff --git a/models/link_3.stl b/models/link_3.stl
new file mode 100644
index 0000000..39ca154
Binary files /dev/null and b/models/link_3.stl differ
diff --git a/models/link_4.stl b/models/link_4.stl
new file mode 100644
index 0000000..26f365b
Binary files /dev/null and b/models/link_4.stl differ
diff --git a/models/link_5.stl b/models/link_5.stl
new file mode 100644
index 0000000..0d7bf18
Binary files /dev/null and b/models/link_5.stl differ
diff --git a/models/link_6.stl b/models/link_6.stl
new file mode 100644
index 0000000..65a98c2
Binary files /dev/null and b/models/link_6.stl differ
diff --git a/models/link_7.stl b/models/link_7.stl
new file mode 100644
index 0000000..2d7a872
Binary files /dev/null and b/models/link_7.stl differ
diff --git a/models/link_8.stl b/models/link_8.stl
new file mode 100644
index 0000000..2190c00
Binary files /dev/null and b/models/link_8.stl differ
diff --git a/models/link_9.stl b/models/link_9.stl
new file mode 100644
index 0000000..b86f550
Binary files /dev/null and b/models/link_9.stl differ
diff --git a/models/manual_robot.urdf b/models/manual_robot.urdf
index 5e7126a..02657c1 100644
--- a/models/manual_robot.urdf
+++ b/models/manual_robot.urdf
@@ -55,7 +55,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -64,7 +64,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -118,7 +118,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -126,7 +126,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -179,7 +179,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -187,7 +187,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -235,7 +235,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -243,7 +243,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -287,7 +287,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -295,7 +295,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
@@ -327,14 +327,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
-
+
@@ -358,14 +358,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
-
+
@@ -389,14 +389,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
-
+
@@ -420,14 +420,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
-
+
@@ -457,14 +457,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
-
+
-
+
diff --git a/src/config_loader.py b/src/config_loader.py
index e69de29..d8a59ab 100644
--- a/src/config_loader.py
+++ b/src/config_loader.py
@@ -0,0 +1,154 @@
+import json
+import os
+from typing import Dict, Any, Optional
+
+
+class ConfigLoader:
+ _instance: Optional['ConfigLoader'] = None
+ _config: Optional[Dict[str, Any]] = None
+
+ def __new__(cls, config_path: str = "config.json"):
+ if cls._instance is None:
+ cls._instance = super().__new__(cls)
+ return cls._instance
+
+ def __init__(self, config_path: str = "config.json"):
+ if self._config is None:
+ self.config_path = config_path
+ self.load_config()
+ self.validate_config()
+
+ def load_config(self) -> None:
+ if not os.path.exists(self.config_path):
+ raise FileNotFoundError(f"Configuration file not found: {self.config_path}")
+
+ try:
+ with open(self.config_path, 'r', encoding='utf-8') as f:
+ self._config = json.load(f)
+ except json.JSONDecodeError as e:
+ raise ValueError(f"Invalid JSON format in config file: {e}")
+ except Exception as e:
+ raise RuntimeError(f"Failed to load config file: {e}")
+
+ def validate_config(self) -> None:
+ required_sections = ['robot', 'wall', 'hole', 'task_points', 'transport_object', 'simulation']
+
+ for section in required_sections:
+ if section not in self._config:
+ raise ValueError(f"Missing required config section: {section}")
+
+ self._validate_robot_config()
+ self._validate_wall_config()
+ self._validate_hole_config()
+ self._validate_task_points()
+ self._validate_transport_object_config()
+ self._validate_simulation_config()
+
+ def _validate_robot_config(self) -> None:
+ robot_config = self._config['robot']
+
+ if 'model_path' not in robot_config:
+ raise ValueError("Robot config missing model_path")
+
+ model_path = robot_config['model_path']
+ if not os.path.exists(model_path):
+ raise FileNotFoundError(f"Robot model file not found: {model_path}")
+
+ if 'base_position' not in robot_config or len(robot_config['base_position']) != 3:
+ raise ValueError("Robot base_position must contain 3 coordinate values")
+
+ if 'base_orientation' not in robot_config or len(robot_config['base_orientation']) != 3:
+ raise ValueError("Robot base_orientation must contain 3 angle values")
+
+ def _validate_wall_config(self) -> None:
+ wall_config = self._config['wall']
+
+ if 'position' not in wall_config or len(wall_config['position']) != 3:
+ raise ValueError("Wall position must contain 3 coordinate values")
+
+ if 'dimensions' not in wall_config:
+ raise ValueError("Wall config missing dimensions")
+
+ dimensions = wall_config['dimensions']
+ required_dims = ['width', 'height', 'thickness']
+ for dim in required_dims:
+ if dim not in dimensions:
+ raise ValueError(f"Wall dimensions missing {dim}")
+ if dimensions[dim] <= 0:
+ raise ValueError(f"Wall {dim} must be greater than 0")
+
+ def _validate_hole_config(self) -> None:
+ hole_config = self._config['hole']
+
+ if 'position_relative_to_wall' not in hole_config or len(hole_config['position_relative_to_wall']) != 3:
+ raise ValueError("Hole position_relative_to_wall must contain 3 coordinate values")
+
+ if 'dimensions' not in hole_config:
+ raise ValueError("Hole config missing dimensions")
+
+ dimensions = hole_config['dimensions']
+ for dim in ['width', 'height']:
+ if dim not in dimensions:
+ raise ValueError(f"Hole dimensions missing {dim}")
+ if dimensions[dim] <= 0:
+ raise ValueError(f"Hole {dim} must be greater than 0")
+
+ def _validate_task_points(self) -> None:
+ task_points = self._config['task_points']
+
+ for point_name in ['point_A', 'point_B']:
+ if point_name not in task_points:
+ raise ValueError(f"Task points missing {point_name}")
+
+ point = task_points[point_name]
+ if 'position' not in point or len(point['position']) != 3:
+ raise ValueError(f"{point_name} position must contain 3 coordinate values")
+
+ def _validate_transport_object_config(self) -> None:
+ obj_config = self._config['transport_object']
+
+ if 'initial_position' not in obj_config or len(obj_config['initial_position']) != 3:
+ raise ValueError("Transport object initial_position must contain 3 coordinate values")
+
+ if 'dimensions' not in obj_config:
+ raise ValueError("Transport object config missing dimensions")
+
+ dimensions = obj_config['dimensions']
+ for dim in ['width', 'height', 'depth']:
+ if dim not in dimensions:
+ raise ValueError(f"Transport object dimensions missing {dim}")
+ if dimensions[dim] <= 0:
+ raise ValueError(f"Transport object {dim} must be greater than 0")
+
+ if 'mass' not in obj_config or obj_config['mass'] <= 0:
+ raise ValueError("Transport object mass must be greater than 0")
+
+ def _validate_simulation_config(self) -> None:
+ sim_config = self._config['simulation']
+
+ if 'timestep' not in sim_config or sim_config['timestep'] <= 0:
+ raise ValueError("Simulation timestep must be greater than 0")
+
+ if 'gravity' not in sim_config or len(sim_config['gravity']) != 3:
+ raise ValueError("Simulation gravity must contain 3 components")
+
+ def get_robot_config(self) -> Dict[str, Any]:
+ return self._config['robot'].copy()
+
+ def get_wall_config(self) -> Dict[str, Any]:
+ return self._config['wall'].copy()
+
+ def get_hole_config(self) -> Dict[str, Any]:
+ return self._config['hole'].copy()
+
+ def get_task_points(self) -> Dict[str, Any]:
+ return self._config['task_points'].copy()
+
+ def get_transport_object_config(self) -> Dict[str, Any]:
+ return self._config['transport_object'].copy()
+
+ def get_simulation_config(self) -> Dict[str, Any]:
+ return self._config['simulation'].copy()
+
+ def get_full_config(self) -> Dict[str, Any]:
+ return self._config.copy()
\ No newline at end of file
diff --git a/src/gui/__init__.py b/src/gui/__init__.py
new file mode 100644
index 0000000..37b7fd9
--- /dev/null
+++ b/src/gui/__init__.py
@@ -0,0 +1 @@
+# GUI module initialization
\ No newline at end of file
diff --git a/src/gui/config_window.py b/src/gui/config_window.py
new file mode 100644
index 0000000..2340f41
--- /dev/null
+++ b/src/gui/config_window.py
@@ -0,0 +1,400 @@
+import tkinter as tk
+from tkinter import ttk, messagebox
+import json
+from typing import Dict, Any, Callable, Tuple, List
+
+
+class ConfigWindow:
+ # UI Constants extracted from hardcoded values
+ WINDOW_GEOMETRY = "800x600"
+ BUTTON_COLORS = {
+ "apply": "#4CAF50",
+ "save": "#2196F3",
+ "reset": "#f44336"
+ }
+ FONT_TITLE = ("Arial", 12, "bold")
+ FONT_SECTION = ("Arial", 10, "bold")
+ PADDING_STANDARD = 5
+ PADDING_LARGE = 10
+ ENTRY_WIDTH_STANDARD = 10
+ ENTRY_WIDTH_LONG = 40
+ BUTTON_PADDING = 20
+
+ def __init__(self, parent, config_path: str = "config.json", on_apply_callback: Callable = None):
+ self.parent = parent
+ self.config_path = config_path
+ self.on_apply_callback = on_apply_callback
+ self.window = None
+ self.config_data = {}
+ self.entry_widgets = {}
+
+ # Load current configuration
+ self.load_config()
+
+ def load_config(self):
+ """Load configuration from file"""
+ try:
+ with open(self.config_path, 'r', encoding='utf-8') as f:
+ self.config_data = json.load(f)
+ except Exception as e:
+ # Follow fail-fast principle - raise exception instead of hiding it
+ raise RuntimeError(f"Failed to load configuration from {self.config_path}: {e}")
+
+ def save_config(self):
+ """Save configuration to file"""
+ try:
+ with open(self.config_path, 'w', encoding='utf-8') as f:
+ json.dump(self.config_data, f, indent=2, ensure_ascii=False)
+ return True
+ except Exception as e:
+ # Log error but allow UI to handle it
+ raise RuntimeError(f"Failed to save configuration to {self.config_path}: {e}")
+
+ def show(self):
+ """Show configuration window"""
+ if self.window and self.window.winfo_exists():
+ self.window.lift()
+ return
+
+ self.window = tk.Toplevel(self.parent)
+ self.window.title("Configuration Management")
+ self.window.geometry(self.WINDOW_GEOMETRY)
+
+ # Create notebook for tabs
+ notebook = ttk.Notebook(self.window)
+ notebook.pack(fill=tk.BOTH, expand=True, padx=self.PADDING_LARGE, pady=self.PADDING_LARGE)
+
+ # Create tabs
+ self.create_robot_tab(notebook)
+ self.create_wall_tab(notebook)
+ self.create_hole_tab(notebook)
+ self.create_task_points_tab(notebook)
+ self.create_transport_object_tab(notebook)
+ self.create_simulation_tab(notebook)
+
+ # Create buttons frame
+ button_frame = tk.Frame(self.window)
+ button_frame.pack(fill=tk.X, padx=self.PADDING_LARGE, pady=self.PADDING_STANDARD)
+
+ # Apply button
+ apply_btn = tk.Button(button_frame, text="Apply", command=self.apply_changes,
+ bg=self.BUTTON_COLORS["apply"], fg="white", padx=self.BUTTON_PADDING)
+ apply_btn.pack(side=tk.LEFT, padx=self.PADDING_STANDARD)
+
+ # Save button
+ save_btn = tk.Button(button_frame, text="Save to File", command=self.save_changes,
+ bg=self.BUTTON_COLORS["save"], fg="white", padx=self.BUTTON_PADDING)
+ save_btn.pack(side=tk.LEFT, padx=self.PADDING_STANDARD)
+
+ # Reset button
+ reset_btn = tk.Button(button_frame, text="Reset", command=self.reset_values,
+ bg=self.BUTTON_COLORS["reset"], fg="white", padx=self.BUTTON_PADDING)
+ reset_btn.pack(side=tk.LEFT, padx=self.PADDING_STANDARD)
+
+ # Close button
+ close_btn = tk.Button(button_frame, text="Close", command=self.window.destroy,
+ padx=self.BUTTON_PADDING)
+ close_btn.pack(side=tk.RIGHT, padx=self.PADDING_STANDARD)
+
+ def _create_vector_input(self, parent: tk.Frame, row: int, label: str, key_prefix: str,
+ values: List[float], labels: List[str] = None) -> None:
+ """Create vector input fields (x, y, z or custom labels)"""
+ if labels is None:
+ labels = ["X:", "Y:", "Z:"]
+
+ tk.Label(parent, text=label).grid(row=row, column=0, sticky="e",
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+
+ for i, (lbl, val) in enumerate(zip(labels, values)):
+ tk.Label(parent, text=lbl).grid(row=row + i, column=1, sticky="e",
+ padx=self.PADDING_STANDARD)
+ entry_key = f"{key_prefix}.{lbl.lower().rstrip(':')}"
+ self.entry_widgets[entry_key] = tk.Entry(parent, width=self.ENTRY_WIDTH_STANDARD)
+ self.entry_widgets[entry_key].grid(row=row + i, column=2,
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.entry_widgets[entry_key].insert(0, str(val))
+
+ def _create_title_label(self, parent: tk.Frame, text: str, row: int = 0,
+ font_type: str = "title") -> None:
+ """Create a title label"""
+ font = self.FONT_TITLE if font_type == "title" else self.FONT_SECTION
+ tk.Label(parent, text=text, font=font).grid(
+ row=row, column=0, columnspan=6, pady=self.PADDING_LARGE)
+
+ def _get_config_value(self, *keys, default=None):
+ """Safely get nested configuration value"""
+ value = self.config_data
+ for key in keys:
+ if isinstance(value, dict):
+ value = value.get(key, {})
+ else:
+ return default
+ return value if value != {} else default
+
+ def create_robot_tab(self, notebook):
+ """Create robot configuration tab"""
+ frame = ttk.Frame(notebook)
+ notebook.add(frame, text="Robot")
+
+ # Title
+ self._create_title_label(frame, "Robot Configuration")
+
+ # Model path
+ tk.Label(frame, text="Model Path:").grid(row=1, column=0, sticky="e",
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.entry_widgets['robot.model_path'] = tk.Entry(frame, width=self.ENTRY_WIDTH_LONG)
+ self.entry_widgets['robot.model_path'].grid(row=1, column=1, columnspan=3,
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.entry_widgets['robot.model_path'].insert(0, self._get_config_value('robot', 'model_path', default=''))
+
+ # Base position
+ base_pos = self._get_config_value('robot', 'base_position', default=[0, 0, 0])
+ self._create_vector_input(frame, 2, "Base Position:", "robot.base_position", base_pos)
+
+ # Base orientation
+ base_orient = self._get_config_value('robot', 'base_orientation', default=[0, 0, 0])
+ self._create_vector_input(frame, 5, "Base Orientation:", "robot.base_orientation",
+ base_orient, labels=["Roll:", "Pitch:", "Yaw:"])
+
+ def _create_dimension_inputs(self, parent: tk.Frame, row: int, key_prefix: str,
+ dimensions: Dict[str, float], dim_names: List[str]) -> None:
+ """Create dimension input fields"""
+ tk.Label(parent, text="Dimensions:").grid(row=row, column=0, sticky="e",
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+
+ for i, dim_name in enumerate(dim_names):
+ tk.Label(parent, text=f"{dim_name.capitalize()}:").grid(row=row + i, column=1, sticky="e",
+ padx=self.PADDING_STANDARD)
+ entry_key = f"{key_prefix}.{dim_name}"
+ self.entry_widgets[entry_key] = tk.Entry(parent, width=self.ENTRY_WIDTH_STANDARD)
+ self.entry_widgets[entry_key].grid(row=row + i, column=2,
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ # Get default value from config or use 0
+ default_val = dimensions.get(dim_name, 0.0)
+ self.entry_widgets[entry_key].insert(0, str(default_val))
+
+ def create_wall_tab(self, notebook):
+ """Create wall configuration tab"""
+ frame = ttk.Frame(notebook)
+ notebook.add(frame, text="Wall")
+
+ # Title
+ self._create_title_label(frame, "Wall Configuration")
+
+ # Wall position
+ wall_pos = self._get_config_value('wall', 'position', default=[2.0, 0.0, 1.0])
+ self._create_vector_input(frame, 1, "Position:", "wall.position", wall_pos)
+
+ # Wall dimensions
+ wall_dims = self._get_config_value('wall', 'dimensions', default={})
+ # Set defaults if not in config
+ wall_dims.setdefault('width', 3.0)
+ wall_dims.setdefault('height', 2.5)
+ wall_dims.setdefault('thickness', 0.2)
+ self._create_dimension_inputs(frame, 4, "wall.dimensions", wall_dims,
+ ['width', 'height', 'thickness'])
+
+ def create_hole_tab(self, notebook):
+ """Create hole configuration tab"""
+ frame = ttk.Frame(notebook)
+ notebook.add(frame, text="Hole")
+
+ # Title
+ self._create_title_label(frame, "Hole Configuration")
+
+ # Hole position relative to wall
+ hole_pos = self._get_config_value('hole', 'position_relative_to_wall', default=[0, 0, 0])
+ self._create_vector_input(frame, 1, "Position (relative):", "hole.position", hole_pos)
+
+ # Hole dimensions
+ hole_dims = self._get_config_value('hole', 'dimensions', default={})
+ hole_dims.setdefault('width', 0.6)
+ hole_dims.setdefault('height', 0.6)
+ self._create_dimension_inputs(frame, 4, "hole.dimensions", hole_dims, ['width', 'height'])
+
+ def _create_horizontal_vector_input(self, parent: tk.Frame, row: int, label: str,
+ key_prefix: str, values: List[float]) -> None:
+ """Create horizontal vector input fields (for compact layout)"""
+ labels = ["X:", "Y:", "Z:"]
+
+ for i, (lbl, val) in enumerate(zip(labels, values)):
+ tk.Label(parent, text=lbl).grid(row=row, column=i*2, sticky="e",
+ padx=self.PADDING_STANDARD)
+ entry_key = f"{key_prefix}.{lbl.lower().rstrip(':')}"
+ self.entry_widgets[entry_key] = tk.Entry(parent, width=self.ENTRY_WIDTH_STANDARD)
+ self.entry_widgets[entry_key].grid(row=row, column=i*2+1,
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.entry_widgets[entry_key].insert(0, str(val))
+
+ def create_task_points_tab(self, notebook):
+ """Create task points configuration tab"""
+ frame = ttk.Frame(notebook)
+ notebook.add(frame, text="Task Points")
+
+ # Title
+ self._create_title_label(frame, "Task Points Configuration")
+
+ # Point A
+ self._create_title_label(frame, "Point A (Pick-up):", row=1, font_type="section")
+
+ point_a = self._get_config_value('task_points', 'point_A', 'position',
+ default=[1.0, 0.5, 0.8])
+ self._create_horizontal_vector_input(frame, 2, "Point A", "point_a", point_a)
+
+ # Point B
+ self._create_title_label(frame, "Point B (Drop-off):", row=3, font_type="section")
+
+ point_b = self._get_config_value('task_points', 'point_B', 'position',
+ default=[3.0, 0.5, 0.8])
+ self._create_horizontal_vector_input(frame, 4, "Point B", "point_b", point_b)
+
+ def create_transport_object_tab(self, notebook):
+ """Create transport object configuration tab"""
+ frame = ttk.Frame(notebook)
+ notebook.add(frame, text="Transport Object")
+
+ # Title
+ self._create_title_label(frame, "Transport Object Configuration")
+
+ # Initial position
+ obj_pos = self._get_config_value('transport_object', 'initial_position',
+ default=[1.0, 0.5, 0.5])
+ self._create_vector_input(frame, 1, "Initial Position:", "object.position", obj_pos)
+
+ # Dimensions
+ obj_dims = self._get_config_value('transport_object', 'dimensions', default={})
+ obj_dims.setdefault('width', 0.1)
+ obj_dims.setdefault('height', 0.1)
+ obj_dims.setdefault('depth', 0.1)
+ self._create_dimension_inputs(frame, 4, "object.dimensions", obj_dims,
+ ['width', 'height', 'depth'])
+
+ # Mass
+ mass = self._get_config_value('transport_object', 'mass', default=0.5)
+ tk.Label(frame, text="Mass (kg):").grid(row=7, column=0, sticky="e",
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.entry_widgets['object.mass'] = tk.Entry(frame, width=self.ENTRY_WIDTH_STANDARD)
+ self.entry_widgets['object.mass'].grid(row=7, column=2,
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.entry_widgets['object.mass'].insert(0, str(mass))
+
+ def create_simulation_tab(self, notebook):
+ """Create simulation configuration tab"""
+ frame = ttk.Frame(notebook)
+ notebook.add(frame, text="Simulation")
+
+ # Title
+ self._create_title_label(frame, "Simulation Configuration")
+
+ # Timestep
+ timestep = self._get_config_value('simulation', 'timestep', default=0.01)
+ tk.Label(frame, text="Timestep:").grid(row=1, column=0, sticky="e",
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.entry_widgets['simulation.timestep'] = tk.Entry(frame, width=self.ENTRY_WIDTH_STANDARD)
+ self.entry_widgets['simulation.timestep'].grid(row=1, column=1,
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.entry_widgets['simulation.timestep'].insert(0, str(timestep))
+
+ # Gravity
+ gravity = self._get_config_value('simulation', 'gravity', default=[0, 0, -9.81])
+ self._create_vector_input(frame, 2, "Gravity:", "simulation.gravity", gravity)
+
+ # Real-time mode
+ real_time = self._get_config_value('simulation', 'real_time', default=False)
+ tk.Label(frame, text="Real-time Mode:").grid(row=5, column=0, sticky="e",
+ padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD)
+ self.real_time_var = tk.BooleanVar()
+ self.real_time_var.set(real_time)
+ tk.Checkbutton(frame, variable=self.real_time_var).grid(row=5, column=1,
+ padx=self.PADDING_STANDARD,
+ pady=self.PADDING_STANDARD, sticky="w")
+
+ def _get_vector_from_widgets(self, key_prefix: str) -> List[float]:
+ """Get vector values from widgets"""
+ return [
+ float(self.entry_widgets[f'{key_prefix}.x'].get()),
+ float(self.entry_widgets[f'{key_prefix}.y'].get()),
+ float(self.entry_widgets[f'{key_prefix}.z'].get())
+ ]
+
+ def _get_vector_custom_from_widgets(self, key_prefix: str, labels: List[str]) -> List[float]:
+ """Get vector values from widgets with custom labels"""
+ return [float(self.entry_widgets[f'{key_prefix}.{label}'].get()) for label in labels]
+
+ def _ensure_nested_dict(self, *keys) -> None:
+ """Ensure nested dictionary structure exists"""
+ current = self.config_data
+ for key in keys:
+ if key not in current:
+ current[key] = {}
+ current = current[key]
+
+ def get_values_from_widgets(self) -> Dict[str, Any]:
+ """Get values from all entry widgets and update config_data"""
+ try:
+ # Ensure all necessary nested structures exist
+ self._ensure_nested_dict('robot')
+ self._ensure_nested_dict('wall', 'dimensions')
+ self._ensure_nested_dict('hole', 'dimensions')
+ self._ensure_nested_dict('task_points', 'point_A')
+ self._ensure_nested_dict('task_points', 'point_B')
+ self._ensure_nested_dict('transport_object', 'dimensions')
+ self._ensure_nested_dict('simulation')
+
+ # Robot configuration
+ self.config_data['robot']['model_path'] = self.entry_widgets['robot.model_path'].get()
+ self.config_data['robot']['base_position'] = self._get_vector_from_widgets('robot.base_position')
+ self.config_data['robot']['base_orientation'] = self._get_vector_custom_from_widgets(
+ 'robot.base_orientation', ['roll', 'pitch', 'yaw'])
+
+ # Wall configuration
+ self.config_data['wall']['position'] = self._get_vector_from_widgets('wall.position')
+ for dim in ['width', 'height', 'thickness']:
+ self.config_data['wall']['dimensions'][dim] = float(
+ self.entry_widgets[f'wall.dimensions.{dim}'].get())
+
+ # Hole configuration
+ self.config_data['hole']['position_relative_to_wall'] = self._get_vector_from_widgets('hole.position')
+ for dim in ['width', 'height']:
+ self.config_data['hole']['dimensions'][dim] = float(
+ self.entry_widgets[f'hole.dimensions.{dim}'].get())
+
+ # Task points
+ self.config_data['task_points']['point_A']['position'] = self._get_vector_from_widgets('point_a')
+ self.config_data['task_points']['point_B']['position'] = self._get_vector_from_widgets('point_b')
+
+ # Transport object
+ self.config_data['transport_object']['initial_position'] = self._get_vector_from_widgets('object.position')
+ for dim in ['width', 'height', 'depth']:
+ self.config_data['transport_object']['dimensions'][dim] = float(
+ self.entry_widgets[f'object.dimensions.{dim}'].get())
+ self.config_data['transport_object']['mass'] = float(self.entry_widgets['object.mass'].get())
+
+ # Simulation
+ self.config_data['simulation']['timestep'] = float(self.entry_widgets['simulation.timestep'].get())
+ self.config_data['simulation']['gravity'] = self._get_vector_from_widgets('simulation.gravity')
+ self.config_data['simulation']['real_time'] = self.real_time_var.get()
+
+ return self.config_data
+ except (ValueError, KeyError) as e:
+ # Allow UI to handle the error display
+ raise ValueError(f"Invalid input or configuration structure: {e}")
+
+ def apply_changes(self):
+ """Apply configuration changes"""
+ config = self.get_values_from_widgets()
+ if self.on_apply_callback:
+ self.on_apply_callback(config)
+
+ def save_changes(self):
+ """Save configuration changes to file"""
+ self.config_data = self.get_values_from_widgets()
+ self.save_config()
+
+ def reset_values(self):
+ """Reset values to original configuration"""
+ self.load_config()
+ # Refresh window
+ if self.window:
+ self.window.destroy()
+ self.show()
\ No newline at end of file
diff --git a/src/gui/main_window.py b/src/gui/main_window.py
new file mode 100644
index 0000000..affbb96
--- /dev/null
+++ b/src/gui/main_window.py
@@ -0,0 +1,434 @@
+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.robot_loader import RobotLoader
+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.robot_loader = 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")
+
+ # Load robot
+ self.robot_loader = RobotLoader(self.config_loader, self.physics_client)
+ self.robot_loader.load_robot()
+ 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.robot_loader and self.robot_loader.robot_id is not None:
+ robot_state = self.robot_loader.get_joint_states()
+
+ # Clear current environment
+ if self.environment:
+ self.environment.cleanup()
+ if self.robot_loader and self.robot_loader.robot_id is not None:
+ p.removeBody(self.robot_loader.robot_id, physicsClientId=self.physics_client)
+
+ # 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()
+
+ # Reload robot
+ self.robot_loader = RobotLoader(self.config_loader, self.physics_client)
+ self.robot_loader.load_robot()
+
+ # Restore robot state if available
+ if robot_state:
+ try:
+ positions = [state['position'] for state in robot_state.values()]
+ self.robot_loader.set_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
+ if self.robot_loader:
+ self.robot_loader.reset_robot()
+
+ # 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
+ self._check_reachability(point_a, point_b)
+
+ except Exception as e:
+ self.update_status(f"Reachability test failed: {e}")
+ raise
+
+ def _check_reachability(self, point_a, point_b):
+ """Check if points are reachable by the robot"""
+ # Calculate distances
+ distance_a = self._calculate_distance(point_a)
+ distance_b = self._calculate_distance(point_b)
+
+ # Get robot reach
+ ee_pos, _ = self.robot_loader.get_end_effector_pose()
+ current_reach = self._calculate_distance(ee_pos)
+
+ # Check reachability
+ a_reachable = distance_a <= current_reach
+ b_reachable = distance_b <= current_reach
+
+ if a_reachable and b_reachable:
+ self.update_status("Both points are reachable")
+ elif not a_reachable:
+ self.update_status(f"Point A out of reach ({distance_a:.2f}m > {current_reach:.2f}m)")
+ elif not b_reachable:
+ self.update_status(f"Point B out of reach ({distance_b:.2f}m > {current_reach:.2f}m)")
+
+ 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()
\ No newline at end of file
diff --git a/src/robot/kinematics.py b/src/robot/kinematics.py
index e69de29..4160323 100644
--- a/src/robot/kinematics.py
+++ b/src/robot/kinematics.py
@@ -0,0 +1,347 @@
+#!/usr/bin/env python3
+"""
+KDL-based Kinematics Engine
+
+Provides forward and inverse kinematics calculations using KDL library.
+Supports URDF-based robot configurations for precise kinematic computations.
+"""
+
+import PyKDL as kdl
+import numpy as np
+from typing import List, Tuple, Optional, Dict
+from urdf_parser_py.urdf import URDF
+import sys
+import os
+
+
+class KinematicsEngine:
+ """KDL-based kinematics engine for robot arm calculations"""
+
+ def __init__(self, urdf_path: str, base_link: str, end_link: str,
+ base_position: List[float] = None, base_orientation: List[float] = None):
+ """Initialize KDL kinematics engine from URDF file"""
+ self.urdf_path = urdf_path
+ self.base_link = base_link
+ self.end_link = end_link
+
+ # Base coordinate transformation (base pose in world frame)
+ self.base_position = base_position if base_position else [0.0, 0.0, 0.0]
+ self.base_orientation = base_orientation if base_orientation else [0.0, 0.0, 0.0] # RPY
+ self._base_to_world_frame = self._create_transform_frame(self.base_position, self.base_orientation)
+ self._world_to_base_frame = self._base_to_world_frame.Inverse()
+
+ # KDL components
+ self.chain: Optional[kdl.Chain] = None
+ self.fk_solver: Optional[kdl.ChainFkSolverPos_recursive] = None
+ self.ik_solver: Optional[kdl.ChainIkSolverPos_NR_JL] = None
+ self.jac_solver: Optional[kdl.ChainJntToJacSolver] = None
+ self.joint_limits: List[Tuple[float, float]] = []
+
+ # Initialize KDL chain and solvers
+ self._build_kdl_chain()
+ self._create_solvers()
+
+ def _build_kdl_chain(self) -> None:
+ """Build KDL chain from URDF file"""
+ try:
+ # Parse URDF
+ robot = URDF.from_xml_file(self.urdf_path)
+
+ # Build chain from base to end effector
+ self.chain = self._urdf_to_kdl_chain(robot, self.base_link, self.end_link)
+
+ if self.chain is None:
+ raise RuntimeError(f"Failed to build KDL chain from {self.base_link} to {self.end_link}")
+
+ except Exception as e:
+ raise RuntimeError(f"Failed to build KDL chain: {e}")
+
+ def _urdf_to_kdl_chain(self, robot: URDF, base_link: str, end_link: str) -> kdl.Chain:
+ """Convert URDF robot to KDL chain"""
+ chain = kdl.Chain()
+
+ # Find path from base to end link
+ path = self._find_link_path(robot, base_link, end_link)
+ if not path:
+ raise RuntimeError(f"No path found from {base_link} to {end_link}")
+
+ # Build chain along the path
+ for i in range(len(path) - 1):
+ current_link = path[i]
+ next_link = path[i + 1]
+
+ # Find joint connecting these links
+ joint = self._find_joint_between_links(robot, current_link, next_link)
+ if joint is None:
+ continue
+
+ # Convert joint to KDL segment
+ segment = self._joint_to_kdl_segment(joint, robot)
+ chain.addSegment(segment)
+
+ # Store joint limits for controllable joints
+ if joint.type in ['revolute', 'prismatic']:
+ if joint.limit:
+ self.joint_limits.append((joint.limit.lower, joint.limit.upper))
+ else:
+ # Default large limits if not specified
+ self.joint_limits.append((-3.14159, 3.14159))
+
+ return chain
+
+ def _find_link_path(self, robot: URDF, start_link: str, end_link: str) -> List[str]:
+ """Find path between two links in URDF tree"""
+ if start_link == end_link:
+ return [start_link]
+
+ # Build parent-child relationships
+ parent_map = {}
+ for joint in robot.joints:
+ parent_map[joint.child] = joint.parent
+
+ # Find path from end to start
+ path = [end_link]
+ current = end_link
+
+ while current != start_link and current in parent_map:
+ current = parent_map[current]
+ path.append(current)
+
+ if current != start_link:
+ return [] # No path found
+
+ # Reverse to get start->end path
+ return list(reversed(path))
+
+ def _find_joint_between_links(self, robot: URDF, parent_link: str, child_link: str):
+ """Find joint connecting two links"""
+ for joint in robot.joints:
+ if joint.parent == parent_link and joint.child == child_link:
+ return joint
+ return None
+
+ def _joint_to_kdl_segment(self, joint, robot: URDF) -> kdl.Segment:
+ """Convert URDF joint to KDL segment"""
+ # Get joint origin
+ origin = joint.origin if joint.origin else URDF.Origin()
+ xyz = origin.xyz if origin.xyz else [0, 0, 0]
+ rpy = origin.rpy if origin.rpy else [0, 0, 0]
+
+ # Create KDL frame
+ rotation = kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2])
+ translation = kdl.Vector(xyz[0], xyz[1], xyz[2])
+ frame = kdl.Frame(rotation, translation)
+
+ # Create KDL joint
+ if joint.type == 'revolute':
+ axis = joint.axis if joint.axis else [0, 0, 1]
+ kdl_joint = kdl.Joint(joint.name, kdl.Joint.RotZ if axis == [0, 0, 1]
+ else kdl.Joint.RotY if axis == [0, 1, 0]
+ else kdl.Joint.RotX)
+ elif joint.type == 'prismatic':
+ axis = joint.axis if joint.axis else [0, 0, 1]
+ kdl_joint = kdl.Joint(joint.name, kdl.Joint.TransZ if axis == [0, 0, 1]
+ else kdl.Joint.TransY if axis == [0, 1, 0]
+ else kdl.Joint.TransX)
+ else:
+ # Fixed joint
+ kdl_joint = kdl.Joint(joint.name, kdl.Joint.None)
+
+ return kdl.Segment(joint.child, kdl_joint, frame)
+
+ def _create_solvers(self) -> None:
+ """Create KDL solvers for forward kinematics, inverse kinematics and Jacobian"""
+ if self.chain is None:
+ raise RuntimeError("KDL chain not initialized")
+
+ try:
+ # Forward kinematics solver
+ self.fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
+
+ # Jacobian solver
+ self.jac_solver = kdl.ChainJntToJacSolver(self.chain)
+
+ # Inverse kinematics solver with joint limits
+ if self.joint_limits:
+ q_min = kdl.JntArray(len(self.joint_limits))
+ q_max = kdl.JntArray(len(self.joint_limits))
+
+ for i, (lower, upper) in enumerate(self.joint_limits):
+ q_min[i] = lower
+ q_max[i] = upper
+
+ # Create velocity IK solver first
+ ik_vel_solver = kdl.ChainIkSolverVel_pinv(self.chain)
+
+ # Create position IK solver with joint limits
+ self.ik_solver = kdl.ChainIkSolverPos_NR_JL(
+ self.chain, q_min, q_max, self.fk_solver, ik_vel_solver
+ )
+ else:
+ # Fallback without joint limits
+ ik_vel_solver = kdl.ChainIkSolverVel_pinv(self.chain)
+ self.ik_solver = kdl.ChainIkSolverPos_NR(
+ self.chain, self.fk_solver, ik_vel_solver
+ )
+
+ except Exception as e:
+ raise RuntimeError(f"Failed to create KDL solvers: {e}")
+
+ def forward_kinematics(self, joint_positions: List[float]) -> Tuple[List[float], List[float]]:
+ """Calculate forward kinematics from joint positions to world coordinate end effector pose"""
+ if self.fk_solver is None:
+ raise RuntimeError("Forward kinematics solver not initialized")
+
+ if len(joint_positions) != self.chain.getNrOfJoints():
+ raise ValueError(f"Expected {self.chain.getNrOfJoints()} joint positions, got {len(joint_positions)}")
+
+ # Create joint array
+ q = kdl.JntArray(len(joint_positions))
+ for i, pos in enumerate(joint_positions):
+ q[i] = pos
+
+ # Solve forward kinematics in base frame
+ base_end_frame = kdl.Frame()
+ result = self.fk_solver.JntToCart(q, base_end_frame)
+
+ if result != 0:
+ raise RuntimeError("Forward kinematics calculation failed")
+
+ # Transform from base frame to world frame
+ world_end_frame = self._base_to_world_frame * base_end_frame
+
+ # Extract world position
+ position = [
+ world_end_frame.p.x(),
+ world_end_frame.p.y(),
+ world_end_frame.p.z()
+ ]
+
+ # Extract world orientation as quaternion
+ rotation = world_end_frame.M
+ orientation = self._rotation_to_quaternion(rotation)
+
+ return position, orientation
+
+ def inverse_kinematics(self, target_position: List[float],
+ target_orientation: List[float] = None,
+ seed_angles: List[float] = None) -> Optional[List[float]]:
+ """Calculate inverse kinematics from world coordinate target pose to joint positions"""
+ if self.ik_solver is None:
+ raise RuntimeError("Inverse kinematics solver not initialized")
+
+ # Create world target frame
+ world_target_frame = kdl.Frame()
+ world_target_frame.p = kdl.Vector(target_position[0], target_position[1], target_position[2])
+
+ if target_orientation is not None:
+ world_target_frame.M = self._quaternion_to_rotation(target_orientation)
+
+ # Transform from world frame to base frame
+ base_target_frame = self._world_to_base_frame * world_target_frame
+
+ # Set seed angles
+ q_init = kdl.JntArray(self.chain.getNrOfJoints())
+ if seed_angles:
+ if len(seed_angles) != self.chain.getNrOfJoints():
+ raise ValueError(f"Expected {self.chain.getNrOfJoints()} seed angles, got {len(seed_angles)}")
+ for i, angle in enumerate(seed_angles):
+ q_init[i] = angle
+ else:
+ # Use zero as default seed
+ for i in range(self.chain.getNrOfJoints()):
+ q_init[i] = 0.0
+
+ # Solve inverse kinematics in base frame
+ q_out = kdl.JntArray(self.chain.getNrOfJoints())
+ result = self.ik_solver.CartToJnt(q_init, base_target_frame, q_out)
+
+ if result < 0:
+ return None # No solution found
+
+ # Return joint angles
+ joint_angles = []
+ for i in range(q_out.rows()):
+ joint_angles.append(q_out[i])
+
+ return joint_angles
+
+ def compute_jacobian(self, joint_positions: List[float]) -> np.ndarray:
+ """Compute Jacobian matrix for given joint configuration"""
+ if self.jac_solver is None:
+ raise RuntimeError("Jacobian solver not initialized")
+
+ if len(joint_positions) != self.chain.getNrOfJoints():
+ raise ValueError(f"Expected {self.chain.getNrOfJoints()} joint positions, got {len(joint_positions)}")
+
+ # Create joint array
+ q = kdl.JntArray(len(joint_positions))
+ for i, pos in enumerate(joint_positions):
+ q[i] = pos
+
+ # Compute Jacobian
+ jacobian = kdl.Jacobian(self.chain.getNrOfJoints())
+ result = self.jac_solver.JntToJac(q, jacobian)
+
+ if result != 0:
+ raise RuntimeError("Jacobian calculation failed")
+
+ # Convert to numpy array
+ jac_array = np.zeros((6, self.chain.getNrOfJoints()))
+ for i in range(6):
+ for j in range(self.chain.getNrOfJoints()):
+ jac_array[i, j] = jacobian[i, j]
+
+ return jac_array
+
+ def get_joint_limits(self) -> List[Tuple[float, float]]:
+ """Get joint limits for all joints in the chain"""
+ return self.joint_limits.copy()
+
+ def validate_joint_configuration(self, joint_positions: List[float]) -> bool:
+ """Validate if joint configuration is within limits"""
+ if len(joint_positions) != len(self.joint_limits):
+ return False
+
+ for i, (pos, (lower, upper)) in enumerate(zip(joint_positions, self.joint_limits)):
+ if pos < lower or pos > upper:
+ return False
+
+ return True
+
+ def get_num_joints(self) -> int:
+ """Get number of joints in the kinematic chain"""
+ return self.chain.getNrOfJoints() if self.chain else 0
+
+ def _rotation_to_quaternion(self, rotation: kdl.Rotation) -> List[float]:
+ """Convert KDL rotation to quaternion [x, y, z, w]"""
+ # Get quaternion from KDL rotation
+ quat = rotation.GetQuaternion()
+ return [quat[0], quat[1], quat[2], quat[3]] # [x, y, z, w]
+
+ def _quaternion_to_rotation(self, quaternion: List[float]) -> kdl.Rotation:
+ """Convert quaternion [x, y, z, w] to KDL rotation"""
+ return kdl.Rotation.Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])
+
+ def _create_transform_frame(self, position: List[float], orientation_rpy: List[float]) -> kdl.Frame:
+ """Create KDL frame from position and RPY orientation"""
+ rotation = kdl.Rotation.RPY(orientation_rpy[0], orientation_rpy[1], orientation_rpy[2])
+ translation = kdl.Vector(position[0], position[1], position[2])
+ return kdl.Frame(rotation, translation)
+
+
+def create_kinematics_engine(config_loader) -> KinematicsEngine:
+ """Create KinematicsEngine from configuration loader"""
+ robot_config = config_loader.get_robot_config()
+ urdf_path = robot_config['model_path']
+
+ # Get robot base pose from config
+ base_position = robot_config.get('base_position', [0.0, 0.0, 0.0])
+ base_orientation = robot_config.get('base_orientation', [0.0, 0.0, 0.0])
+
+ # Get link names from config or use defaults
+ kinematics_config = robot_config.get('kinematics', {})
+ base_link = kinematics_config.get('base_link', 'base_link')
+ end_link = kinematics_config.get('end_link', 'link_9')
+
+ return KinematicsEngine(urdf_path, base_link, end_link, base_position, base_orientation)
\ No newline at end of file
diff --git a/src/robot/robot_loader.py b/src/robot/robot_loader.py
new file mode 100644
index 0000000..5a40bd6
--- /dev/null
+++ b/src/robot/robot_loader.py
@@ -0,0 +1,304 @@
+import pybullet as p
+import os
+from typing import Dict, List, Tuple, Any, Optional
+from ..config_loader import ConfigLoader
+
+
+class RobotLoader:
+ def __init__(self, config_loader: ConfigLoader, physics_client: int):
+ self.config_loader = config_loader
+ self.physics_client = physics_client
+ self.robot_id: Optional[int] = None
+ self.num_joints: int = 0
+ self.joint_indices: List[int] = []
+ self.joint_info: Dict[int, Dict[str, Any]] = {}
+ self.link_info: Dict[int, Dict[str, Any]] = {}
+ self.joint_name_to_index: Dict[str, int] = {}
+ self.end_effector_index: Optional[int] = None
+
+ # Get robot configuration
+ self.robot_config = config_loader.get_robot_config()
+ self.base_position = self.robot_config['base_position']
+ self.base_orientation = self.robot_config['base_orientation']
+
+ # Get expected DOF from config if available
+ self.expected_dof = self.robot_config.get('dof', None)
+
+ def load_robot(self) -> int:
+ """Load robot URDF model into pybullet simulation"""
+ model_path = self.robot_config['model_path']
+
+ if not os.path.exists(model_path):
+ raise FileNotFoundError(f"Robot URDF file not found: {model_path}")
+
+ # Convert RPY to quaternion for pybullet
+ base_orientation_quat = p.getQuaternionFromEuler(self.base_orientation)
+
+ # Load URDF model
+ self.robot_id = p.loadURDF(
+ model_path,
+ basePosition=self.base_position,
+ baseOrientation=base_orientation_quat,
+ physicsClientId=self.physics_client
+ )
+
+ if self.robot_id is None:
+ raise RuntimeError(f"Failed to load robot URDF: {model_path}")
+
+ # Parse robot structure
+ self._parse_robot_structure()
+
+ # Validate robot DOF if specified in config
+ if self.expected_dof is not None:
+ if len(self.joint_indices) != self.expected_dof:
+ raise ValueError(f"Expected {self.expected_dof} DOF robot, found {len(self.joint_indices)} controllable joints")
+
+ # Set initial joint positions if available in config
+ self.reset_robot()
+
+ return self.robot_id
+
+ def _parse_robot_structure(self) -> None:
+ """Parse robot structure and extract joint/link information"""
+ self.num_joints = p.getNumJoints(self.robot_id, physicsClientId=self.physics_client)
+
+ self.joint_indices = []
+ self.joint_info = {}
+ self.link_info = {}
+ self.joint_name_to_index = {}
+
+ for joint_index in range(self.num_joints):
+ joint_info = p.getJointInfo(self.robot_id, joint_index, physicsClientId=self.physics_client)
+
+ joint_name = joint_info[1].decode('utf-8')
+ joint_type = joint_info[2]
+
+ # Store joint information
+ self.joint_info[joint_index] = {
+ 'name': joint_name,
+ 'type': joint_type,
+ 'lower_limit': joint_info[8],
+ 'upper_limit': joint_info[9],
+ 'max_force': joint_info[10],
+ 'max_velocity': joint_info[11],
+ 'axis': joint_info[13]
+ }
+
+ self.joint_name_to_index[joint_name] = joint_index
+
+ # Only include revolute and prismatic joints as controllable
+ if joint_type in [p.JOINT_REVOLUTE, p.JOINT_PRISMATIC]:
+ self.joint_indices.append(joint_index)
+
+ # Find end effector (last link)
+ if self.num_joints > 0:
+ self.end_effector_index = self.num_joints - 1
+
+ def get_joint_states(self) -> Dict[str, Dict[str, float]]:
+ """Get current states of all controllable joints"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ joint_states = {}
+
+ for joint_index in self.joint_indices:
+ state = p.getJointState(self.robot_id, joint_index, physicsClientId=self.physics_client)
+ joint_name = self.joint_info[joint_index]['name']
+
+ joint_states[joint_name] = {
+ 'position': state[0],
+ 'velocity': state[1],
+ 'reaction_forces': state[2],
+ 'applied_torque': state[3]
+ }
+
+ return joint_states
+
+ def get_joint_limits(self) -> Dict[str, Tuple[float, float]]:
+ """Get joint limits for all controllable joints"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ joint_limits = {}
+
+ for joint_index in self.joint_indices:
+ joint_name = self.joint_info[joint_index]['name']
+ lower_limit = self.joint_info[joint_index]['lower_limit']
+ upper_limit = self.joint_info[joint_index]['upper_limit']
+
+ joint_limits[joint_name] = (lower_limit, upper_limit)
+
+ return joint_limits
+
+ def get_end_effector_pose(self) -> Tuple[List[float], List[float]]:
+ """Get end effector pose (position and orientation)"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ if self.end_effector_index is None:
+ raise RuntimeError("End effector not identified")
+
+ link_state = p.getLinkState(
+ self.robot_id,
+ self.end_effector_index,
+ physicsClientId=self.physics_client
+ )
+
+ position = list(link_state[0])
+ orientation = list(link_state[1])
+
+ return position, orientation
+
+ def get_link_state(self, link_index: int) -> Dict[str, Any]:
+ """Get state of a specific link"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ link_state = p.getLinkState(
+ self.robot_id,
+ link_index,
+ physicsClientId=self.physics_client
+ )
+
+ return {
+ 'position': list(link_state[0]),
+ 'orientation': list(link_state[1]),
+ 'local_inertial_position': list(link_state[2]),
+ 'local_inertial_orientation': list(link_state[3]),
+ 'world_position': list(link_state[4]),
+ 'world_orientation': list(link_state[5])
+ }
+
+ def set_joint_positions(self, joint_positions: List[float]) -> None:
+ """Set target positions for all controllable joints"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ if len(joint_positions) != len(self.joint_indices):
+ raise ValueError(f"Expected {len(self.joint_indices)} joint positions, got {len(joint_positions)}")
+
+ # Validate joint positions
+ if not self.validate_joint_positions(joint_positions):
+ raise ValueError("Joint positions exceed limits")
+
+ # Set joint positions
+ for i, joint_index in enumerate(self.joint_indices):
+ p.setJointMotorControl2(
+ self.robot_id,
+ joint_index,
+ p.POSITION_CONTROL,
+ targetPosition=joint_positions[i],
+ physicsClientId=self.physics_client
+ )
+
+ def set_joint_velocities(self, joint_velocities: List[float]) -> None:
+ """Set target velocities for all controllable joints"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ if len(joint_velocities) != len(self.joint_indices):
+ raise ValueError(f"Expected {len(self.joint_indices)} joint velocities, got {len(joint_velocities)}")
+
+ # Set joint velocities
+ for i, joint_index in enumerate(self.joint_indices):
+ p.setJointMotorControl2(
+ self.robot_id,
+ joint_index,
+ p.VELOCITY_CONTROL,
+ targetVelocity=joint_velocities[i],
+ physicsClientId=self.physics_client
+ )
+
+ def reset_robot(self) -> None:
+ """Reset robot to initial configuration"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ # Get initial positions from config or use zeros
+ initial_positions = self.robot_config.get('initial_joint_positions', [0.0] * len(self.joint_indices))
+
+ # Ensure initial positions list matches joint count
+ if len(initial_positions) != len(self.joint_indices):
+ initial_positions = [0.0] * len(self.joint_indices)
+
+ for i, joint_index in enumerate(self.joint_indices):
+ p.resetJointState(
+ self.robot_id,
+ joint_index,
+ initial_positions[i],
+ physicsClientId=self.physics_client
+ )
+
+ def validate_joint_positions(self, positions: List[float]) -> bool:
+ """Validate if joint positions are within limits"""
+ if len(positions) != len(self.joint_indices):
+ return False
+
+ for i, joint_index in enumerate(self.joint_indices):
+ lower_limit = self.joint_info[joint_index]['lower_limit']
+ upper_limit = self.joint_info[joint_index]['upper_limit']
+
+ # Skip validation for unlimited joints
+ if lower_limit == 0.0 and upper_limit == -1.0:
+ continue
+
+ if positions[i] < lower_limit or positions[i] > upper_limit:
+ return False
+
+ return True
+
+ def check_self_collision(self) -> bool:
+ """Check if robot has self-collision"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ # Get contact points for self-collision detection
+ contact_points = p.getContactPoints(
+ bodyA=self.robot_id,
+ bodyB=self.robot_id,
+ physicsClientId=self.physics_client
+ )
+
+ return len(contact_points) > 0
+
+ def get_joint_names(self) -> List[str]:
+ """Get names of all controllable joints"""
+ return [self.joint_info[idx]['name'] for idx in self.joint_indices]
+
+ def get_joint_info(self) -> Dict[int, Dict[str, Any]]:
+ """Get complete joint information"""
+ return self.joint_info.copy()
+
+ def get_robot_id(self) -> Optional[int]:
+ """Get pybullet robot ID"""
+ return self.robot_id
+
+ def get_dof(self) -> int:
+ """Get number of degrees of freedom"""
+ return len(self.joint_indices)
+
+ def enable_torque_control(self) -> None:
+ """Switch to torque control mode"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ for joint_index in self.joint_indices:
+ p.setJointMotorControl2(
+ self.robot_id,
+ joint_index,
+ p.TORQUE_CONTROL,
+ force=0,
+ physicsClientId=self.physics_client
+ )
+
+ def enable_position_control(self) -> None:
+ """Switch to position control mode"""
+ if self.robot_id is None:
+ raise RuntimeError("Robot not loaded. Call load_robot() first.")
+
+ # Get current positions
+ current_states = self.get_joint_states()
+ current_positions = [state['position'] for state in current_states.values()]
+
+ # Set position control with current positions as targets
+ self.set_joint_positions(current_positions)
\ No newline at end of file
diff --git a/src/simulation/environment.py b/src/simulation/environment.py
index e69de29..fa853af 100644
--- a/src/simulation/environment.py
+++ b/src/simulation/environment.py
@@ -0,0 +1,317 @@
+import pybullet as p
+import pybullet_data
+from typing import Dict, List, Any, Optional, Tuple
+from ..config_loader import ConfigLoader
+
+
+class Environment:
+ def __init__(self, config_loader: ConfigLoader, physics_client: int):
+ self.config_loader = config_loader
+ self.physics_client = physics_client
+
+ # Get configurations
+ self.wall_config = config_loader.get_wall_config()
+ self.hole_config = config_loader.get_hole_config()
+ self.transport_object_config = config_loader.get_transport_object_config()
+ self.simulation_config = config_loader.get_simulation_config()
+
+ # Environment object IDs
+ self.wall_id: Optional[int] = None
+ self.transport_object_id: Optional[int] = None
+ self.ground_plane_id: Optional[int] = None
+
+ def setup_simulation(self) -> None:
+ """Setup basic simulation environment"""
+ # Set additional search path for pybullet data
+ p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+ # Set gravity
+ gravity = self.simulation_config['gravity']
+ p.setGravity(gravity[0], gravity[1], gravity[2], physicsClientId=self.physics_client)
+
+ # Set time step
+ timestep = self.simulation_config['timestep']
+ p.setTimeStep(timestep, physicsClientId=self.physics_client)
+
+ # Set real time simulation
+ real_time = self.simulation_config.get('real_time', False)
+ if real_time:
+ p.setRealTimeSimulation(1, physicsClientId=self.physics_client)
+ else:
+ p.setRealTimeSimulation(0, physicsClientId=self.physics_client)
+
+ def create_ground_plane(self) -> int:
+ """Create ground plane"""
+ self.ground_plane_id = p.loadURDF(
+ "plane.urdf",
+ physicsClientId=self.physics_client
+ )
+ return self.ground_plane_id
+
+ def create_wall_with_hole(self) -> int:
+ """Create wall with hole obstacle"""
+ wall_pos = self.wall_config['position']
+ wall_dims = self.wall_config['dimensions']
+ hole_dims = self.hole_config['dimensions']
+ hole_pos_rel = self.hole_config['position_relative_to_wall']
+
+ # Calculate wall dimensions
+ wall_width = wall_dims['width']
+ wall_height = wall_dims['height']
+ wall_thickness = wall_dims['thickness']
+
+ # Calculate hole dimensions
+ hole_width = hole_dims['width']
+ hole_height = hole_dims['height']
+
+ # Calculate absolute hole position
+ hole_pos = [
+ wall_pos[0] + hole_pos_rel[0],
+ wall_pos[1] + hole_pos_rel[1],
+ wall_pos[2] + hole_pos_rel[2]
+ ]
+
+ # Create wall with hole using multiple boxes
+ wall_parts = []
+
+ # Wall parameters
+ wall_color = self.wall_config.get('material', {}).get('color', [0.8, 0.8, 0.8, 1.0])
+
+ # Bottom part (below hole)
+ bottom_height = hole_pos[2] - wall_pos[2] - hole_height / 2
+ if bottom_height > 0:
+ bottom_pos = [wall_pos[0], wall_pos[1], wall_pos[2] + bottom_height / 2]
+ bottom_id = self._create_box(
+ [wall_width / 2, wall_thickness / 2, bottom_height / 2],
+ bottom_pos,
+ wall_color
+ )
+ wall_parts.append(bottom_id)
+
+ # Top part (above hole)
+ top_height = wall_pos[2] + wall_height - (hole_pos[2] + hole_height / 2)
+ if top_height > 0:
+ top_pos = [wall_pos[0], wall_pos[1], hole_pos[2] + hole_height / 2 + top_height / 2]
+ top_id = self._create_box(
+ [wall_width / 2, wall_thickness / 2, top_height / 2],
+ top_pos,
+ wall_color
+ )
+ wall_parts.append(top_id)
+
+ # Left part (left of hole)
+ left_width = hole_pos[0] - wall_pos[0] - hole_width / 2 + wall_width / 2
+ if left_width > 0:
+ left_pos = [wall_pos[0] - wall_width / 2 + left_width / 2, wall_pos[1], hole_pos[2]]
+ left_id = self._create_box(
+ [left_width / 2, wall_thickness / 2, hole_height / 2],
+ left_pos,
+ wall_color
+ )
+ wall_parts.append(left_id)
+
+ # Right part (right of hole)
+ right_width = wall_pos[0] + wall_width / 2 - (hole_pos[0] + hole_width / 2)
+ if right_width > 0:
+ right_pos = [hole_pos[0] + hole_width / 2 + right_width / 2, wall_pos[1], hole_pos[2]]
+ right_id = self._create_box(
+ [right_width / 2, wall_thickness / 2, hole_height / 2],
+ right_pos,
+ wall_color
+ )
+ wall_parts.append(right_id)
+
+ # Store the first wall part as main wall ID
+ if wall_parts:
+ self.wall_id = wall_parts[0]
+
+ return wall_parts
+
+ def _create_box(self, half_extents: List[float], position: List[float], color: List[float]) -> int:
+ """Create a box collision shape and body"""
+ # Create collision shape
+ collision_shape = p.createCollisionShape(
+ p.GEOM_BOX,
+ halfExtents=half_extents,
+ physicsClientId=self.physics_client
+ )
+
+ # Create visual shape
+ visual_shape = p.createVisualShape(
+ p.GEOM_BOX,
+ halfExtents=half_extents,
+ rgbaColor=color,
+ physicsClientId=self.physics_client
+ )
+
+ # Create multi-body
+ body_id = p.createMultiBody(
+ baseMass=0, # Static object
+ baseCollisionShapeIndex=collision_shape,
+ baseVisualShapeIndex=visual_shape,
+ basePosition=position,
+ physicsClientId=self.physics_client
+ )
+
+ return body_id
+
+ def create_transport_object(self) -> int:
+ """Create the object to be transported"""
+ obj_config = self.transport_object_config
+
+ # Get object parameters
+ initial_pos = obj_config['initial_position']
+ dimensions = obj_config['dimensions']
+ mass = obj_config['mass']
+ shape = obj_config.get('shape', 'cube')
+ color = obj_config.get('material', {}).get('color', [1.0, 0.0, 0.0, 1.0])
+
+ if shape == 'cube':
+ # Create cube
+ half_extents = [
+ dimensions['width'] / 2,
+ dimensions['height'] / 2,
+ dimensions['depth'] / 2
+ ]
+
+ collision_shape = p.createCollisionShape(
+ p.GEOM_BOX,
+ halfExtents=half_extents,
+ physicsClientId=self.physics_client
+ )
+
+ visual_shape = p.createVisualShape(
+ p.GEOM_BOX,
+ halfExtents=half_extents,
+ rgbaColor=color,
+ physicsClientId=self.physics_client
+ )
+ else:
+ raise ValueError(f"Unsupported transport object shape: {shape}")
+
+ # Create multi-body
+ self.transport_object_id = p.createMultiBody(
+ baseMass=mass,
+ baseCollisionShapeIndex=collision_shape,
+ baseVisualShapeIndex=visual_shape,
+ basePosition=initial_pos,
+ physicsClientId=self.physics_client
+ )
+
+ return self.transport_object_id
+
+ def setup_environment(self) -> Dict[str, Any]:
+ """Setup complete environment and return object IDs"""
+ # Setup basic simulation parameters
+ self.setup_simulation()
+
+ # Create ground plane
+ ground_id = self.create_ground_plane()
+
+ # Create wall with hole
+ wall_parts = self.create_wall_with_hole()
+
+ # Create transport object
+ transport_obj_id = self.create_transport_object()
+
+ return {
+ 'ground_plane_id': ground_id,
+ 'wall_parts': wall_parts,
+ 'transport_object_id': transport_obj_id
+ }
+
+ def get_transport_object_pose(self) -> Tuple[List[float], List[float]]:
+ """Get current pose of transport object"""
+ if self.transport_object_id is None:
+ raise RuntimeError("Transport object not created")
+
+ pos, orient = p.getBasePositionAndOrientation(
+ self.transport_object_id,
+ physicsClientId=self.physics_client
+ )
+
+ return list(pos), list(orient)
+
+ def set_transport_object_pose(self, position: List[float], orientation: List[float] = None) -> None:
+ """Set pose of transport object"""
+ if self.transport_object_id is None:
+ raise RuntimeError("Transport object not created")
+
+ if orientation is None:
+ orientation = [0, 0, 0, 1] # Default quaternion
+
+ p.resetBasePositionAndOrientation(
+ self.transport_object_id,
+ position,
+ orientation,
+ physicsClientId=self.physics_client
+ )
+
+ def check_collision(self, body_a: int, body_b: int = None) -> bool:
+ """Check collision between bodies"""
+ if body_b is None:
+ # Check collision with any other body
+ contact_points = p.getContactPoints(
+ bodyA=body_a,
+ physicsClientId=self.physics_client
+ )
+ else:
+ # Check collision between specific bodies
+ contact_points = p.getContactPoints(
+ bodyA=body_a,
+ bodyB=body_b,
+ physicsClientId=self.physics_client
+ )
+
+ return len(contact_points) > 0
+
+ def get_hole_info(self) -> Dict[str, Any]:
+ """Get hole information for path planning"""
+ wall_pos = self.wall_config['position']
+ hole_pos_rel = self.hole_config['position_relative_to_wall']
+ hole_dims = self.hole_config['dimensions']
+
+ # Calculate absolute hole position
+ hole_center = [
+ wall_pos[0] + hole_pos_rel[0],
+ wall_pos[1] + hole_pos_rel[1],
+ wall_pos[2] + hole_pos_rel[2]
+ ]
+
+ return {
+ 'center': hole_center,
+ 'dimensions': hole_dims,
+ 'shape': self.hole_config.get('shape', 'rectangle')
+ }
+
+ def get_wall_info(self) -> Dict[str, Any]:
+ """Get wall information for path planning"""
+ return {
+ 'position': self.wall_config['position'],
+ 'dimensions': self.wall_config['dimensions']
+ }
+
+ def reset_environment(self) -> None:
+ """Reset environment to initial state"""
+ if self.transport_object_id is not None:
+ initial_pos = self.transport_object_config['initial_position']
+ self.set_transport_object_pose(initial_pos)
+
+ def cleanup(self) -> None:
+ """Clean up environment objects"""
+ objects_to_remove = []
+
+ if self.wall_id is not None:
+ objects_to_remove.append(self.wall_id)
+
+ if self.transport_object_id is not None:
+ objects_to_remove.append(self.transport_object_id)
+
+ if self.ground_plane_id is not None:
+ objects_to_remove.append(self.ground_plane_id)
+
+ for obj_id in objects_to_remove:
+ try:
+ p.removeBody(obj_id, physicsClientId=self.physics_client)
+ except:
+ pass # Object might already be removed
\ No newline at end of file
diff --git a/tests/__init__.py b/tests/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/tests/test_config_loader.py b/tests/test_config_loader.py
new file mode 100644
index 0000000..6108408
--- /dev/null
+++ b/tests/test_config_loader.py
@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+
+import sys
+import os
+sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from src.config_loader import ConfigLoader
+
+def test_config_loader():
+ print("Testing ConfigLoader...")
+
+ # Test singleton pattern
+ loader1 = ConfigLoader()
+ loader2 = ConfigLoader()
+ assert loader1 is loader2, "ConfigLoader should be singleton"
+ print("✓ Singleton pattern works")
+
+ # Test config loading
+ config = loader1.get_full_config()
+ assert 'robot' in config, "Config should contain robot section"
+ assert 'wall' in config, "Config should contain wall section"
+ assert 'hole' in config, "Config should contain hole section"
+ assert 'task_points' in config, "Config should contain task_points section"
+ assert 'transport_object' in config, "Config should contain transport_object section"
+ assert 'simulation' in config, "Config should contain simulation section"
+ print("✓ All required config sections present")
+
+ # Test robot config
+ robot_config = loader1.get_robot_config()
+ assert robot_config['model_path'] == "models/manual_robot.urdf", "Robot model path should be correct"
+ assert len(robot_config['base_position']) == 3, "Robot base position should have 3 coordinates"
+ assert len(robot_config['base_orientation']) == 3, "Robot base orientation should have 3 angles"
+ print("✓ Robot config validation passed")
+
+ # Test wall config
+ wall_config = loader1.get_wall_config()
+ assert len(wall_config['position']) == 3, "Wall position should have 3 coordinates"
+ assert 'dimensions' in wall_config, "Wall should have dimensions"
+ assert wall_config['dimensions']['width'] > 0, "Wall width should be positive"
+ assert wall_config['dimensions']['height'] > 0, "Wall height should be positive"
+ assert wall_config['dimensions']['thickness'] > 0, "Wall thickness should be positive"
+ print("✓ Wall config validation passed")
+
+ # Test hole config
+ hole_config = loader1.get_hole_config()
+ assert len(hole_config['position_relative_to_wall']) == 3, "Hole position should have 3 coordinates"
+ assert 'dimensions' in hole_config, "Hole should have dimensions"
+ assert hole_config['dimensions']['width'] > 0, "Hole width should be positive"
+ assert hole_config['dimensions']['height'] > 0, "Hole height should be positive"
+ print("✓ Hole config validation passed")
+
+ # Test task points
+ task_points = loader1.get_task_points()
+ assert 'point_A' in task_points, "Should have point_A"
+ assert 'point_B' in task_points, "Should have point_B"
+ assert len(task_points['point_A']['position']) == 3, "Point A should have 3 coordinates"
+ assert len(task_points['point_B']['position']) == 3, "Point B should have 3 coordinates"
+ print("✓ Task points validation passed")
+
+ # Test transport object config
+ obj_config = loader1.get_transport_object_config()
+ assert len(obj_config['initial_position']) == 3, "Object initial position should have 3 coordinates"
+ assert 'dimensions' in obj_config, "Object should have dimensions"
+ assert obj_config['mass'] > 0, "Object mass should be positive"
+ print("✓ Transport object config validation passed")
+
+ # Test simulation config
+ sim_config = loader1.get_simulation_config()
+ assert sim_config['timestep'] > 0, "Timestep should be positive"
+ assert len(sim_config['gravity']) == 3, "Gravity should have 3 components"
+ print("✓ Simulation config validation passed")
+
+ print("\n✅ All tests passed! ConfigLoader is working correctly.")
+
+ # Print sample data
+ print("\nSample configuration data:")
+ print(f"Robot model: {robot_config['model_path']}")
+ print(f"Wall position: {wall_config['position']}")
+ print(f"Wall dimensions: {wall_config['dimensions']}")
+ print(f"Hole dimensions: {hole_config['dimensions']}")
+ print(f"Point A: {task_points['point_A']['position']}")
+ print(f"Point B: {task_points['point_B']['position']}")
+ print(f"Object mass: {obj_config['mass']} kg")
+ print(f"Simulation timestep: {sim_config['timestep']} s")
+
+if __name__ == "__main__":
+ test_config_loader()
\ No newline at end of file
diff --git a/tests/test_environment.py b/tests/test_environment.py
new file mode 100644
index 0000000..15dba0a
--- /dev/null
+++ b/tests/test_environment.py
@@ -0,0 +1,94 @@
+#!/usr/bin/env python3
+
+import sys
+import os
+sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+import pybullet as p
+from src.config_loader import ConfigLoader
+from src.simulation.environment import Environment
+
+def test_environment():
+ print("Testing Environment...")
+
+ # Initialize pybullet in DIRECT mode for testing
+ physics_client = p.connect(p.DIRECT)
+
+ try:
+ # Test environment initialization
+ config_loader = ConfigLoader()
+ environment = Environment(config_loader, physics_client)
+ print("✓ Environment initialization successful")
+
+ # Test environment setup
+ env_objects = environment.setup_environment()
+ print("✓ Environment setup successful")
+
+ # Verify all objects were created
+ assert 'ground_plane_id' in env_objects, "Ground plane should be created"
+ assert 'wall_parts' in env_objects, "Wall parts should be created"
+ assert 'transport_object_id' in env_objects, "Transport object should be created"
+ print(f"✓ Created objects: ground={env_objects['ground_plane_id']}, wall_parts={len(env_objects['wall_parts'])}, object={env_objects['transport_object_id']}")
+
+ # Test transport object pose
+ obj_pos, obj_orient = environment.get_transport_object_pose()
+ assert len(obj_pos) == 3, "Object position should have 3 coordinates"
+ assert len(obj_orient) == 4, "Object orientation should have 4 quaternion values"
+ print(f"✓ Transport object pose: pos={obj_pos}, orient={obj_orient}")
+
+ # Test setting object pose
+ new_pos = [1.5, 0.0, 0.5]
+ environment.set_transport_object_pose(new_pos)
+ updated_pos, _ = environment.get_transport_object_pose()
+ assert abs(updated_pos[0] - new_pos[0]) < 0.01, "Object position should be updated"
+ print("✓ Transport object pose setting works")
+
+ # Test hole info
+ hole_info = environment.get_hole_info()
+ assert 'center' in hole_info, "Hole info should include center"
+ assert 'dimensions' in hole_info, "Hole info should include dimensions"
+ assert 'shape' in hole_info, "Hole info should include shape"
+ print(f"✓ Hole info: center={hole_info['center']}, dims={hole_info['dimensions']}")
+
+ # Test wall info
+ wall_info = environment.get_wall_info()
+ assert 'position' in wall_info, "Wall info should include position"
+ assert 'dimensions' in wall_info, "Wall info should include dimensions"
+ print(f"✓ Wall info: pos={wall_info['position']}, dims={wall_info['dimensions']}")
+
+ # Test environment reset
+ environment.reset_environment()
+ reset_pos, _ = environment.get_transport_object_pose()
+ expected_pos = config_loader.get_transport_object_config()['initial_position']
+ assert abs(reset_pos[0] - expected_pos[0]) < 0.01, "Object should be reset to initial position"
+ print("✓ Environment reset successful")
+
+ # Test collision detection
+ ground_id = env_objects['ground_plane_id']
+ transport_id = env_objects['transport_object_id']
+ has_collision = environment.check_collision(transport_id, ground_id)
+ print(f"✓ Collision detection: {has_collision}")
+
+ # Test cleanup
+ environment.cleanup()
+ print("✓ Environment cleanup successful")
+
+ print("\n✅ All Environment tests passed!")
+
+ # Print environment information
+ print(f"\nEnvironment Information:")
+ print(f"- Wall parts created: {len(env_objects['wall_parts'])}")
+ print(f"- Transport object mass: {config_loader.get_transport_object_config()['mass']} kg")
+ print(f"- Hole dimensions: {hole_info['dimensions']}")
+ print(f"- Wall dimensions: {wall_info['dimensions']}")
+
+ except Exception as e:
+ print(f"❌ Test failed: {e}")
+ raise
+ finally:
+ # Cleanup
+ p.disconnect(physics_client)
+ print("✓ PyBullet disconnected")
+
+if __name__ == "__main__":
+ test_environment()
\ No newline at end of file
diff --git a/tests/test_robot_loader.py b/tests/test_robot_loader.py
new file mode 100644
index 0000000..89619ce
--- /dev/null
+++ b/tests/test_robot_loader.py
@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+
+import sys
+import os
+sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+import pybullet as p
+from src.config_loader import ConfigLoader
+from src.robot.robot_loader import RobotLoader
+
+def test_robot_loader():
+ print("Testing RobotLoader...")
+
+ # Initialize pybullet in DIRECT mode for testing
+ physics_client = p.connect(p.DIRECT)
+
+ try:
+ # Test config loading
+ config_loader = ConfigLoader()
+ robot_loader = RobotLoader(config_loader, physics_client)
+ print("✓ RobotLoader initialization successful")
+
+ # Test robot loading
+ robot_id = robot_loader.load_robot()
+ assert robot_id is not None, "Robot ID should not be None"
+ print(f"✓ Robot loaded successfully with ID: {robot_id}")
+
+ # Test DOF
+ dof = robot_loader.get_dof()
+ print(f"✓ Robot has {dof} degrees of freedom")
+
+ # Test joint names
+ joint_names = robot_loader.get_joint_names()
+ assert len(joint_names) == dof, "Joint names count should match DOF"
+ print(f"✓ Joint names: {joint_names[:3]}..." if len(joint_names) > 3 else f"✓ Joint names: {joint_names}")
+
+ # Test joint limits
+ joint_limits = robot_loader.get_joint_limits()
+ assert len(joint_limits) == dof, "Joint limits count should match DOF"
+ print("✓ Joint limits retrieved successfully")
+
+ # Test joint states
+ joint_states = robot_loader.get_joint_states()
+ assert len(joint_states) == dof, "Joint states count should match DOF"
+ print("✓ Joint states retrieved successfully")
+
+ # Test end effector pose
+ ee_pos, ee_orient = robot_loader.get_end_effector_pose()
+ assert len(ee_pos) == 3, "End effector position should have 3 coordinates"
+ assert len(ee_orient) == 4, "End effector orientation should have 4 quaternion values"
+ print(f"✓ End effector pose: pos={ee_pos}, orient={ee_orient}")
+
+ # Test joint position validation
+ zero_positions = [0.0] * dof
+ is_valid = robot_loader.validate_joint_positions(zero_positions)
+ print(f"✓ Zero positions validation: {is_valid}")
+
+ # Test setting joint positions
+ robot_loader.set_joint_positions(zero_positions)
+ print("✓ Joint positions set successfully")
+
+ # Test robot reset
+ robot_loader.reset_robot()
+ print("✓ Robot reset successful")
+
+ # Test self-collision check
+ has_collision = robot_loader.check_self_collision()
+ print(f"✓ Self-collision check: {has_collision}")
+
+ # Test control mode switching
+ robot_loader.enable_position_control()
+ print("✓ Position control mode enabled")
+
+ robot_loader.enable_torque_control()
+ print("✓ Torque control mode enabled")
+
+ print("\n✅ All RobotLoader tests passed!")
+
+ # Print robot information
+ print(f"\nRobot Information:")
+ print(f"- DOF: {dof}")
+ print(f"- Robot ID: {robot_id}")
+ print(f"- Joint count: {robot_loader.num_joints}")
+ print(f"- Controllable joints: {len(robot_loader.joint_indices)}")
+ print(f"- End effector index: {robot_loader.end_effector_index}")
+
+ except Exception as e:
+ print(f"❌ Test failed: {e}")
+ raise
+ finally:
+ # Cleanup
+ p.disconnect(physics_client)
+ print("✓ PyBullet disconnected")
+
+if __name__ == "__main__":
+ test_robot_loader()
\ No newline at end of file