Implement KDL kinematics engine and complete core framework
Major Features Added: - KDL-based kinematics engine with world/local coordinate transformation - Complete GUI system with configuration management - Robot loader with URDF parsing and joint control - Enhanced environment system with transport object management - Main application entry point with dependency validation Technical Improvements: - World coordinate to robot base coordinate transformation in KDL - Real-time configuration updates through GUI - Comprehensive error handling and validation - Configuration-driven design throughout - Robot model scaling adjusted to 0.2x for proper visualization Files Added: - src/robot/kinematics.py: KDL forward/inverse kinematics with coordinate transforms - src/gui/: Complete GUI framework with main window and config management - main.py: Application entry point with environment validation - models/*.stl: Robot link mesh files for URDF visualization - tests/: Unit test framework for core components This commit establishes the complete foundation for robotic arm path planning and task execution development. 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
parent
a9e936bc43
commit
3b5306611a
80
config.json
80
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"
|
||||
}
|
||||
}
|
||||
88
main.py
Normal file
88
main.py
Normal file
@ -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()
|
||||
BIN
models/link_0.stl
Normal file
BIN
models/link_0.stl
Normal file
Binary file not shown.
BIN
models/link_1.stl
Normal file
BIN
models/link_1.stl
Normal file
Binary file not shown.
BIN
models/link_2.stl
Normal file
BIN
models/link_2.stl
Normal file
Binary file not shown.
BIN
models/link_3.stl
Normal file
BIN
models/link_3.stl
Normal file
Binary file not shown.
BIN
models/link_4.stl
Normal file
BIN
models/link_4.stl
Normal file
Binary file not shown.
BIN
models/link_5.stl
Normal file
BIN
models/link_5.stl
Normal file
Binary file not shown.
BIN
models/link_6.stl
Normal file
BIN
models/link_6.stl
Normal file
Binary file not shown.
BIN
models/link_7.stl
Normal file
BIN
models/link_7.stl
Normal file
Binary file not shown.
BIN
models/link_8.stl
Normal file
BIN
models/link_8.stl
Normal file
Binary file not shown.
BIN
models/link_9.stl
Normal file
BIN
models/link_9.stl
Normal file
Binary file not shown.
@ -55,7 +55,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Mesh position and rotation relative to link coordinate system -->
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_0.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
<mesh filename="link_0.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
</geometry>
|
||||
<material name="orange"/> <!-- Use orange material -->
|
||||
</visual>
|
||||
@ -64,7 +64,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_0.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
<mesh filename="link_0.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
@ -118,7 +118,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- If mesh direction is wrong, can adjust rpy -->
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
<mesh filename="link_1.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
</geometry>
|
||||
<material name="blue"/> <!-- Use blue material -->
|
||||
</visual>
|
||||
@ -126,7 +126,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
<mesh filename="link_1.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
@ -179,7 +179,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<!-- rpy="0 1.57 0" rotate 90 degrees around Y-axis -->
|
||||
<!-- rpy="0 0 1.57" rotate 90 degrees around Z-axis -->
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
<mesh filename="link_2.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
</geometry>
|
||||
<material name="orange"/>
|
||||
</visual>
|
||||
@ -187,7 +187,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
<mesh filename="link_2.stl" scale="0.2 0.2 0.2"/> <!-- Use 0.2x scaled mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
@ -235,7 +235,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_3.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
@ -243,7 +243,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_3.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
@ -287,7 +287,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_4.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
<material name="orange"/>
|
||||
</visual>
|
||||
@ -295,7 +295,7 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_4.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
@ -327,14 +327,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- If need to adjust mesh direction, modify rpy -->
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_5.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_5.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
@ -358,14 +358,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_6.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
<material name="orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_6.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
@ -389,14 +389,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_7.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_7.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
@ -420,14 +420,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_8.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_8.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
<material name="orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_8.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_8.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
@ -457,14 +457,14 @@ Link masses decrease gradually from base to end effector, following robot arm de
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_9.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_9.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_9.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
<mesh filename="link_9.stl" scale="0.2 0.2 0.2"/> <!-- Use original mesh size -->
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
|
||||
@ -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()
|
||||
1
src/gui/__init__.py
Normal file
1
src/gui/__init__.py
Normal file
@ -0,0 +1 @@
|
||||
# GUI module initialization
|
||||
400
src/gui/config_window.py
Normal file
400
src/gui/config_window.py
Normal file
@ -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()
|
||||
434
src/gui/main_window.py
Normal file
434
src/gui/main_window.py
Normal file
@ -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()
|
||||
@ -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)
|
||||
304
src/robot/robot_loader.py
Normal file
304
src/robot/robot_loader.py
Normal file
@ -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)
|
||||
@ -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
|
||||
0
tests/__init__.py
Normal file
0
tests/__init__.py
Normal file
87
tests/test_config_loader.py
Normal file
87
tests/test_config_loader.py
Normal file
@ -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()
|
||||
94
tests/test_environment.py
Normal file
94
tests/test_environment.py
Normal file
@ -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()
|
||||
96
tests/test_robot_loader.py
Normal file
96
tests/test_robot_loader.py
Normal file
@ -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()
|
||||
Loading…
Reference in New Issue
Block a user