refactor: 重构配置管理，遵循单一职责原则
将单文件使用的配置改为文件内常量定义： - kinematics.py: 运动学求解参数改为常量 - ai_rrt_star.py: RRT*算法参数改为常量 - path_optimizer.py: 路径优化参数改为常量 - main_window.py: GUI界面参数改为常量 - hole_crossing.py: 洞口穿越参数改为常量 保留跨文件共享的配置在config.json： - robot, wall, hole, task_points等多模块使用的配置 - path_planning的collision和execution参数 更新CLAUDE.md文档说明新的配置管理原则。 遵循"配置文件用于多文件共享，单文件使用参数定义为文件内常量"原则。 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
parent
2ad7049386
commit
a9db87d81d
17
CLAUDE.md
17
CLAUDE.md
@ -32,14 +32,25 @@ This file provides guidance to Claude Code (claude.ai/code) when working with co
|
||||
|
||||
## 配置管理原则
|
||||
|
||||
**所有配置参数必须从 `config.json` 文件读取，严禁硬编码：**
|
||||
- 机械臂模型路径必须从 `config.robot.model_path` 读取
|
||||
**配置管理遵循单一职责原则：**
|
||||
|
||||
### 跨文件共享配置（存储在 `config.json`）
|
||||
- 机械臂模型路径从 `config.robot.model_path` 读取
|
||||
- 墙体参数从 `config.wall` 读取
|
||||
- 洞口参数从 `config.hole` 读取
|
||||
- 任务点A、B从 `config.task_points` 读取
|
||||
- 运送物体参数从 `config.transport_object` 读取
|
||||
- 仿真参数从 `config.simulation` 读取
|
||||
- 修改任何参数只需修改配置文件，无需改动代码
|
||||
- 路径规划的碰撞检测和执行参数从 `config.path_planning` 读取
|
||||
|
||||
### 单文件专用配置（定义为文件内常量）
|
||||
- **运动学求解参数**：在 `src/robot/kinematics.py` 中定义 `MAX_ITERATIONS`、`EPSILON`
|
||||
- **RRT*算法参数**：在 `src/planning/ai_rrt_star.py` 中定义所有算法常量
|
||||
- **路径优化参数**：在 `src/planning/path_optimizer.py` 中定义优化常量
|
||||
- **GUI界面参数**：在 `src/gui/main_window.py` 中定义界面常量
|
||||
- **洞口穿越参数**：在 `src/planning/hole_crossing.py` 中定义策略常量
|
||||
|
||||
**原则**：配置文件用于多文件共享，单文件使用的参数定义为文件内常量
|
||||
|
||||
## 项目架构
|
||||
|
||||
|
||||
30
config.json
30
config.json
@ -95,41 +95,11 @@
|
||||
],
|
||||
"real_time": false
|
||||
},
|
||||
"camera": {
|
||||
"distance": 4.0,
|
||||
"yaw": 45,
|
||||
"pitch": -30,
|
||||
"target": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
]
|
||||
},
|
||||
"gui": {
|
||||
"window_size": "1200x800"
|
||||
},
|
||||
"kinematics": {
|
||||
"max_iterations": 1500,
|
||||
"epsilon": 1e-05
|
||||
},
|
||||
"path_planning": {
|
||||
"ai_rrt_star": {
|
||||
"enabled": true,
|
||||
"max_iterations": 5000,
|
||||
"step_size": 0.1,
|
||||
"goal_sample_rate": 0.15,
|
||||
"search_radius": 0.5,
|
||||
"hole_bias_rate": 0.3,
|
||||
"enforce_hole_crossing": false
|
||||
},
|
||||
"collision": {
|
||||
"check_resolution": 0.05,
|
||||
"safety_margin": 0.01
|
||||
},
|
||||
"optimization": {
|
||||
"shortcut_iterations": 50,
|
||||
"smoothing_factor": 0.5
|
||||
},
|
||||
"execution": {
|
||||
"velocity_scaling": 1.0,
|
||||
"position_tolerance": 0.01,
|
||||
|
||||
156
debug_coordinates.py
Normal file
156
debug_coordinates.py
Normal file
@ -0,0 +1,156 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
坐标系统调试脚本
|
||||
验证KDL运动学与PyBullet仿真的坐标一致性
|
||||
"""
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
||||
|
||||
from src.config_loader import ConfigLoader
|
||||
from src.robot.arm_controller import create_arm_controller
|
||||
from src.simulation.environment import Environment
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
import numpy as np
|
||||
|
||||
def debug_coordinates():
|
||||
"""调试坐标系统一致性"""
|
||||
print("=== 坐标系统调试分析 ===")
|
||||
|
||||
# 初始化仿真
|
||||
physics_client = p.connect(p.GUI)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
|
||||
try:
|
||||
# 加载配置
|
||||
config_loader = ConfigLoader()
|
||||
config = config_loader.get_full_config()
|
||||
|
||||
print(f"机械臂基座位置: {config['robot']['base_position']}")
|
||||
print(f"机械臂基座朝向: {config['robot']['base_orientation']}")
|
||||
|
||||
# 创建机械臂控制器
|
||||
arm_controller = create_arm_controller(config_loader, physics_client)
|
||||
|
||||
# 创建环境（包括墙体和任务点）
|
||||
environment = Environment(config_loader, physics_client)
|
||||
|
||||
print("\n=== 关节映射信息 ===")
|
||||
kdl_names = arm_controller.kinematics_engine.get_joint_names()
|
||||
pb_names = arm_controller.robot_loader.get_joint_names()
|
||||
print(f"KDL关节顺序: {kdl_names}")
|
||||
print(f"PyBullet关节顺序: {pb_names}")
|
||||
print(f"KDL→PyBullet映射: {arm_controller._kdl_to_pb}")
|
||||
print(f"PyBullet→KDL映射: {arm_controller._pb_to_kdl}")
|
||||
|
||||
print("\n=== 当前机械臂状态测试 ===")
|
||||
|
||||
# 测试1：获取当前关节位置
|
||||
current_joints = arm_controller.get_current_joint_positions()
|
||||
print(f"当前关节位置(KDL顺序): {[f'{x:.3f}' for x in current_joints]}")
|
||||
|
||||
# 测试2：正向运动学 - 计算末端位置
|
||||
fk_position, fk_orientation = arm_controller.forward_kinematics(current_joints)
|
||||
print(f"正向运动学计算的末端位置: {[f'{x:.3f}' for x in fk_position]}")
|
||||
print(f"正向运动学计算的末端姿态: {[f'{x:.3f}' for x in fk_orientation]}")
|
||||
|
||||
# 测试3：直接从PyBullet获取末端位置（验证）
|
||||
robot_id = arm_controller.robot_loader.get_robot_id()
|
||||
end_effector_index = len(pb_names) # 末端执行器通常是最后一个link
|
||||
link_state = p.getLinkState(robot_id, end_effector_index - 1, physicsClientId=physics_client)
|
||||
pb_end_position = link_state[0] # 世界坐标中的位置
|
||||
pb_end_orientation = link_state[1] # 世界坐标中的姿态(四元数)
|
||||
|
||||
print(f"PyBullet直接查询的末端位置: {[f'{x:.3f}' for x in pb_end_position]}")
|
||||
print(f"PyBullet直接查询的末端姿态: {[f'{x:.3f}' for x in pb_end_orientation]}")
|
||||
|
||||
# 测试4：坐标差异分析
|
||||
pos_diff = np.array(fk_position) - np.array(pb_end_position)
|
||||
pos_error = np.linalg.norm(pos_diff)
|
||||
print(f"位置差异: {[f'{x:.4f}' for x in pos_diff]}")
|
||||
print(f"位置误差幅度: {pos_error:.4f} 米")
|
||||
|
||||
if pos_error > 0.01: # 1cm tolerance
|
||||
print("⚠️ 发现显著位置差异！KDL和PyBullet坐标不一致")
|
||||
else:
|
||||
print("✅ 位置一致性良好")
|
||||
|
||||
print("\n=== 逆向运动学测试 ===")
|
||||
|
||||
# 测试5：任务点可达性
|
||||
task_points = config_loader.get_task_points()
|
||||
|
||||
for point_name, point_info in task_points.items():
|
||||
target_pos = point_info['position']
|
||||
print(f"\n测试 {point_name}: {target_pos}")
|
||||
|
||||
# 使用逆运动学求解
|
||||
joint_solution = arm_controller.inverse_kinematics(target_pos, seed_angles=current_joints)
|
||||
|
||||
if joint_solution is None:
|
||||
print(f"❌ {point_name} 不可达")
|
||||
continue
|
||||
else:
|
||||
print(f"✅ {point_name} 可达，关节解: {[f'{x:.3f}' for x in joint_solution]}")
|
||||
|
||||
# 验证：使用求得的关节配置计算正向运动学
|
||||
verify_pos, verify_ori = arm_controller.forward_kinematics(joint_solution)
|
||||
target_error = np.linalg.norm(np.array(verify_pos) - np.array(target_pos))
|
||||
print(f" 验证位置: {[f'{x:.3f}' for x in verify_pos]}")
|
||||
print(f" 目标误差: {target_error:.4f} 米")
|
||||
|
||||
if target_error > 0.01:
|
||||
print(f" ⚠️ 逆运动学验证失败，误差过大")
|
||||
else:
|
||||
print(f" ✅ 逆运动学验证通过")
|
||||
|
||||
print("\n=== 路径执行测试模拟 ===")
|
||||
|
||||
# 测试6：模拟路径执行中的坐标转换
|
||||
test_joint_config = current_joints.copy()
|
||||
test_joint_config[0] += 0.1 # 微调第一个关节
|
||||
|
||||
print(f"测试关节配置(KDL顺序): {[f'{x:.3f}' for x in test_joint_config]}")
|
||||
|
||||
# 计算预期末端位置（通过KDL）
|
||||
expected_pos, _ = arm_controller.forward_kinematics(test_joint_config)
|
||||
print(f"KDL计算预期末端位置: {[f'{x:.3f}' for x in expected_pos]}")
|
||||
|
||||
# 模拟路径执行器的关节设置
|
||||
arm_controller.set_joint_positions(test_joint_config)
|
||||
|
||||
# 等待仿真稳定
|
||||
for _ in range(100):
|
||||
p.stepSimulation(physicsClientId=physics_client)
|
||||
|
||||
# 检查实际到达的位置
|
||||
actual_joints = arm_controller.get_current_joint_positions()
|
||||
actual_pos, _ = arm_controller.get_current_pose()
|
||||
|
||||
print(f"实际关节位置(KDL顺序): {[f'{x:.3f}' for x in actual_joints]}")
|
||||
print(f"实际末端位置: {[f'{x:.3f}' for x in actual_pos]}")
|
||||
|
||||
# 分析执行误差
|
||||
joint_error = np.linalg.norm(np.array(test_joint_config) - np.array(actual_joints))
|
||||
pos_error = np.linalg.norm(np.array(expected_pos) - np.array(actual_pos))
|
||||
|
||||
print(f"关节执行误差: {joint_error:.4f} rad")
|
||||
print(f"位置执行误差: {pos_error:.4f} 米")
|
||||
|
||||
if pos_error > 0.05: # 5cm tolerance
|
||||
print("❌ 发现路径执行误差，坐标转换存在问题")
|
||||
else:
|
||||
print("✅ 路径执行精度良好")
|
||||
|
||||
except Exception as e:
|
||||
print(f"调试过程中发生错误: {e}")
|
||||
import traceback
|
||||
traceback.print_exc()
|
||||
|
||||
finally:
|
||||
p.disconnect(physicsClientId=physics_client)
|
||||
|
||||
if __name__ == "__main__":
|
||||
debug_coordinates()
|
||||
230
debug_execution.py
Normal file
230
debug_execution.py
Normal file
@ -0,0 +1,230 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
路径执行调试脚本
|
||||
专门测试路径执行过程中的坐标对应关系
|
||||
"""
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
||||
|
||||
from src.config_loader import ConfigLoader
|
||||
from src.robot.arm_controller import create_arm_controller
|
||||
from src.simulation.environment import Environment
|
||||
from src.planning.ai_rrt_star import AIRRTStarPlanner
|
||||
from src.planning.collision_checker import CollisionChecker
|
||||
from src.planning.path_executor import PathExecutor
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
def debug_path_execution():
|
||||
"""调试路径执行的具体步骤"""
|
||||
print("=== 路径执行调试分析 ===")
|
||||
|
||||
# 初始化仿真
|
||||
physics_client = p.connect(p.GUI)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
p.setGravity(0, 0, 0) # 关闭重力
|
||||
|
||||
try:
|
||||
# 加载配置和组件
|
||||
config_loader = ConfigLoader()
|
||||
arm_controller = create_arm_controller(config_loader, physics_client)
|
||||
environment = Environment(config_loader, physics_client)
|
||||
collision_checker = CollisionChecker(arm_controller, environment, config_loader)
|
||||
path_executor = PathExecutor(arm_controller, config_loader)
|
||||
|
||||
print("=== 步骤1: 生成简单测试路径 ===")
|
||||
|
||||
# 获取当前位置
|
||||
current_joints = arm_controller.get_current_joint_positions()
|
||||
print(f"起始关节配置: {[f'{x:.3f}' for x in current_joints]}")
|
||||
|
||||
# 计算当前末端位置
|
||||
current_pos, current_ori = arm_controller.forward_kinematics(current_joints)
|
||||
print(f"起始末端位置: {[f'{x:.3f}' for x in current_pos]}")
|
||||
|
||||
# 生成一个简单的测试路径：只修改第一个关节
|
||||
test_path = []
|
||||
for i in range(5): # 5个路径点
|
||||
test_config = current_joints.copy()
|
||||
test_config[0] += i * 0.2 # 每步增加0.2弧度
|
||||
test_path.append(test_config)
|
||||
|
||||
print(f"测试路径包含 {len(test_path)} 个waypoint")
|
||||
|
||||
print("\n=== 步骤2: 分析路径的理论末端位置 ===")
|
||||
theoretical_positions = []
|
||||
for i, config in enumerate(test_path):
|
||||
pos, ori = arm_controller.forward_kinematics(config)
|
||||
theoretical_positions.append(pos)
|
||||
print(f"Waypoint {i}: 关节{config[0]:.3f} -> 末端位置{[f'{x:.3f}' for x in pos]}")
|
||||
|
||||
print("\n=== 步骤3: 可视化理论路径 ===")
|
||||
# 绘制理论路径线条
|
||||
line_ids = []
|
||||
for i in range(len(theoretical_positions) - 1):
|
||||
line_id = p.addUserDebugLine(
|
||||
theoretical_positions[i],
|
||||
theoretical_positions[i + 1],
|
||||
lineColorRGB=[0, 1, 0], # 绿色 - 理论路径
|
||||
lineWidth=3,
|
||||
physicsClientId=physics_client
|
||||
)
|
||||
line_ids.append(line_id)
|
||||
|
||||
print("\n=== 步骤4: 执行路径并记录实际位置 ===")
|
||||
actual_positions = []
|
||||
|
||||
# 重置机械臂到起始位置
|
||||
arm_controller.set_joint_positions(current_joints)
|
||||
for _ in range(50): # 等待稳定
|
||||
p.stepSimulation(physicsClientId=physics_client)
|
||||
|
||||
# 模拟主程序的路径执行器插值方式
|
||||
timestep = 0.01 # 和主程序相同的timestep
|
||||
velocity_scaling = 1.0 # 和主程序默认值相同
|
||||
position_tolerance = 0.01 # 和主程序默认值相同
|
||||
|
||||
for i, target_config in enumerate(test_path):
|
||||
print(f"\n--- 执行Waypoint {i} (模拟路径执行器插值) ---")
|
||||
print(f"目标关节配置: {[f'{x:.3f}' for x in target_config]}")
|
||||
|
||||
# 计算理论末端位置
|
||||
theoretical_pos, _ = arm_controller.forward_kinematics(target_config)
|
||||
print(f"理论末端位置: {[f'{x:.3f}' for x in theoretical_pos]}")
|
||||
|
||||
# === 模拟路径执行器的插值过程 ===
|
||||
current_config = arm_controller.get_current_joint_positions()
|
||||
|
||||
# 计算距离和插值步数
|
||||
distance = np.linalg.norm(np.array(target_config) - np.array(current_config))
|
||||
move_time = distance / velocity_scaling
|
||||
num_steps = int(move_time / timestep) + 1
|
||||
|
||||
print(f" 插值参数: 距离{distance:.4f}, 移动时间{move_time:.3f}s, 插值步数{num_steps}")
|
||||
|
||||
# 执行插值
|
||||
for step in range(1, num_steps + 1):
|
||||
ratio = step / num_steps
|
||||
interpolated = (
|
||||
np.array(current_config) * (1 - ratio) +
|
||||
np.array(target_config) * ratio
|
||||
)
|
||||
|
||||
# 设置插值位置
|
||||
arm_controller.set_joint_positions(interpolated.tolist())
|
||||
|
||||
# 等待timestep时间（和主程序相同）
|
||||
time.sleep(timestep)
|
||||
|
||||
# 记录状态（每10步一次）
|
||||
if step % max(1, num_steps // 5) == 0:
|
||||
actual_joints = arm_controller.get_current_joint_positions()
|
||||
actual_pos, _ = arm_controller.get_current_pose()
|
||||
|
||||
interp_error = np.linalg.norm(interpolated - np.array(actual_joints))
|
||||
pos_error = np.linalg.norm(np.array(theoretical_pos) - np.array(actual_pos))
|
||||
|
||||
print(f" 插值步{step}/{num_steps}: 插值误差{interp_error:.4f}, 位置误差{pos_error:.4f}")
|
||||
|
||||
# 最终设置目标位置
|
||||
arm_controller.set_joint_positions(target_config)
|
||||
|
||||
# 等待收敛（和主程序相同的逻辑）
|
||||
wait_start = time.time()
|
||||
max_wait = max(timestep * 10, move_time * 2)
|
||||
final_converged = False
|
||||
|
||||
while time.time() - wait_start < max_wait:
|
||||
final_config = arm_controller.get_current_joint_positions()
|
||||
final_distance = np.linalg.norm(np.array(target_config) - np.array(final_config))
|
||||
|
||||
if final_distance <= position_tolerance:
|
||||
final_converged = True
|
||||
break
|
||||
time.sleep(timestep)
|
||||
|
||||
# 记录最终实际位置
|
||||
final_joints = arm_controller.get_current_joint_positions()
|
||||
final_pos, _ = arm_controller.get_current_pose()
|
||||
actual_positions.append(final_pos)
|
||||
|
||||
final_joint_error = np.linalg.norm(np.array(target_config) - np.array(final_joints))
|
||||
final_pos_error = np.linalg.norm(np.array(theoretical_pos) - np.array(final_pos))
|
||||
|
||||
print(f"最终关节位置: {[f'{x:.3f}' for x in final_joints]}")
|
||||
print(f"最终末端位置: {[f'{x:.3f}' for x in final_pos]}")
|
||||
print(f"最终关节误差: {final_joint_error:.4f}")
|
||||
print(f"最终位置误差: {final_pos_error:.4f}")
|
||||
|
||||
if final_pos_error > 0.02: # 2cm容差
|
||||
print(f"⚠️ Waypoint {i} 存在显著位置误差！")
|
||||
|
||||
print("\n=== 步骤5: 绘制实际执行路径 ===")
|
||||
# 绘制实际执行路径
|
||||
for i in range(len(actual_positions) - 1):
|
||||
line_id = p.addUserDebugLine(
|
||||
actual_positions[i],
|
||||
actual_positions[i + 1],
|
||||
lineColorRGB=[1, 0, 0], # 红色 - 实际路径
|
||||
lineWidth=3,
|
||||
physicsClientId=physics_client
|
||||
)
|
||||
line_ids.append(line_id)
|
||||
|
||||
print("\n=== 步骤6: 对比分析 ===")
|
||||
total_theoretical_length = 0
|
||||
total_actual_length = 0
|
||||
max_waypoint_error = 0
|
||||
|
||||
for i in range(len(test_path)):
|
||||
theoretical_pos = theoretical_positions[i]
|
||||
actual_pos = actual_positions[i]
|
||||
waypoint_error = np.linalg.norm(np.array(theoretical_pos) - np.array(actual_pos))
|
||||
max_waypoint_error = max(max_waypoint_error, waypoint_error)
|
||||
|
||||
print(f"Waypoint {i}:")
|
||||
print(f" 理论: {[f'{x:.3f}' for x in theoretical_pos]}")
|
||||
print(f" 实际: {[f'{x:.3f}' for x in actual_pos]}")
|
||||
print(f" 误差: {waypoint_error:.4f}m")
|
||||
|
||||
if i > 0:
|
||||
theoretical_segment = np.linalg.norm(
|
||||
np.array(theoretical_positions[i]) - np.array(theoretical_positions[i-1])
|
||||
)
|
||||
actual_segment = np.linalg.norm(
|
||||
np.array(actual_positions[i]) - np.array(actual_positions[i-1])
|
||||
)
|
||||
total_theoretical_length += theoretical_segment
|
||||
total_actual_length += actual_segment
|
||||
|
||||
print(f"\n总结:")
|
||||
print(f"理论路径长度: {total_theoretical_length:.4f}m")
|
||||
print(f"实际路径长度: {total_actual_length:.4f}m")
|
||||
print(f"路径长度差异: {abs(total_theoretical_length - total_actual_length):.4f}m")
|
||||
print(f"最大waypoint误差: {max_waypoint_error:.4f}m")
|
||||
|
||||
if max_waypoint_error > 0.02:
|
||||
print("❌ 发现显著的路径执行误差")
|
||||
else:
|
||||
print("✅ 路径执行精度良好")
|
||||
|
||||
print(f"\n绿色线条 = 理论路径(基于正向运动学)")
|
||||
print(f"红色线条 = 实际执行路径(基于实际机械臂位置)")
|
||||
print(f"如果两条线重合，说明路径执行准确")
|
||||
|
||||
input("按回车键关闭...")
|
||||
|
||||
except Exception as e:
|
||||
print(f"调试过程中发生错误: {e}")
|
||||
import traceback
|
||||
traceback.print_exc()
|
||||
|
||||
finally:
|
||||
p.disconnect(physicsClientId=physics_client)
|
||||
|
||||
if __name__ == "__main__":
|
||||
debug_path_execution()
|
||||
@ -20,6 +20,15 @@ from src.planning.collision_checker import CollisionChecker
|
||||
from src.planning.path_optimizer import PathOptimizer
|
||||
from src.planning.path_executor import PathExecutor
|
||||
|
||||
# GUI设置
|
||||
DEFAULT_WINDOW_SIZE = "1200x800"
|
||||
|
||||
# 相机默认设置
|
||||
DEFAULT_CAMERA_DISTANCE = 4.0
|
||||
DEFAULT_CAMERA_YAW = 45
|
||||
DEFAULT_CAMERA_PITCH = -30
|
||||
DEFAULT_CAMERA_TARGET = [0, 0, 0]
|
||||
|
||||
|
||||
class MainWindow:
|
||||
def __init__(self):
|
||||
@ -56,16 +65,8 @@ class MainWindow:
|
||||
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'])
|
||||
"""Setup window geometry from defaults"""
|
||||
self.root.geometry(DEFAULT_WINDOW_SIZE)
|
||||
|
||||
def _create_control_panel(self):
|
||||
"""Create control panel on the left side"""
|
||||
@ -739,18 +740,13 @@ class MainWindow:
|
||||
self.update_status(f"Path visualization failed: {e}")
|
||||
|
||||
def _reset_camera_view(self):
|
||||
"""Reset camera to default view using config values"""
|
||||
"""Reset camera to default view"""
|
||||
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'])
|
||||
# 使用文件内常量作为默认相机设置
|
||||
distance = DEFAULT_CAMERA_DISTANCE
|
||||
yaw = DEFAULT_CAMERA_YAW
|
||||
pitch = DEFAULT_CAMERA_PITCH
|
||||
target = DEFAULT_CAMERA_TARGET
|
||||
|
||||
p.resetDebugVisualizerCamera(
|
||||
cameraDistance=distance,
|
||||
|
||||
@ -5,7 +5,6 @@ AI-Enhanced RRT* Path Planner
|
||||
AI增强的RRT*路径规划器，通过智能采样、自适应参数和启发式引导
|
||||
提升路径规划效率和成功率，特别优化洞口穿越场景。
|
||||
|
||||
配置驱动：所有参数从config.json读取
|
||||
错误处理：失败立即抛出异常，无后备方案
|
||||
"""
|
||||
|
||||
@ -17,6 +16,15 @@ import math
|
||||
from .collision_checker import CollisionChecker
|
||||
from .hole_crossing import HoleCrossingStrategy
|
||||
|
||||
# RRT*算法参数
|
||||
ENABLED = True
|
||||
MAX_ITERATIONS = 5000
|
||||
STEP_SIZE = 0.1
|
||||
GOAL_SAMPLE_RATE = 0.15
|
||||
SEARCH_RADIUS = 0.5
|
||||
HOLE_BIAS_RATE = 0.3
|
||||
ENFORCE_HOLE_CROSSING = False
|
||||
|
||||
|
||||
class TreeNode:
|
||||
"""RRT*树节点"""
|
||||
@ -39,10 +47,10 @@ class TreeNode:
|
||||
|
||||
class AdaptiveParameters:
|
||||
"""自适应参数管理器"""
|
||||
def __init__(self, config: Dict[str, Any]):
|
||||
self.step_size = config['step_size']
|
||||
self.goal_sample_rate = config['goal_sample_rate']
|
||||
self.search_radius = config['search_radius']
|
||||
def __init__(self):
|
||||
self.step_size = STEP_SIZE
|
||||
self.goal_sample_rate = GOAL_SAMPLE_RATE
|
||||
self.search_radius = SEARCH_RADIUS
|
||||
self.iteration_count = 0
|
||||
self.success_count = 0
|
||||
self.failure_count = 0
|
||||
@ -100,15 +108,11 @@ class AIRRTStarPlanner:
|
||||
self.environment = environment
|
||||
self.config_loader = config_loader
|
||||
|
||||
# 从配置读取参数
|
||||
config = config_loader.get_full_config()
|
||||
self.rrt_config = config['path_planning']['ai_rrt_star']
|
||||
|
||||
# 初始化参数
|
||||
self.max_iterations = self.rrt_config['max_iterations']
|
||||
self.params = AdaptiveParameters(self.rrt_config)
|
||||
# 初始化参数（使用文件内常量）
|
||||
self.max_iterations = MAX_ITERATIONS
|
||||
self.params = AdaptiveParameters()
|
||||
# 是否强制穿洞（默认否）
|
||||
self.enforce_hole = self.rrt_config.get('enforce_hole_crossing', False)
|
||||
self.enforce_hole = ENFORCE_HOLE_CROSSING
|
||||
|
||||
# 初始化组件
|
||||
if collision_checker is None:
|
||||
|
||||
@ -4,14 +4,15 @@ Hole Crossing Strategy Module
|
||||
|
||||
洞口穿越策略模块，提供专门的洞口穿越路径规划策略。
|
||||
包括偏向采样、关键路径点生成、分阶段规划等功能。
|
||||
|
||||
配置驱动设计，所有参数从config.json读取。
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from typing import List, Tuple, Dict, Any, Optional
|
||||
import random
|
||||
|
||||
# 洞口穿越策略参数
|
||||
HOLE_BIAS_RATE = 0.3
|
||||
|
||||
|
||||
class HoleCrossingStrategy:
|
||||
"""洞口穿越策略"""
|
||||
@ -29,9 +30,8 @@ class HoleCrossingStrategy:
|
||||
self.environment = environment
|
||||
self.config_loader = config_loader
|
||||
|
||||
# 从配置读取参数
|
||||
config = config_loader.get_full_config()
|
||||
self.hole_bias_rate = config['path_planning']['ai_rrt_star']['hole_bias_rate']
|
||||
# 洞口策略参数（使用文件内常量）
|
||||
self.hole_bias_rate = HOLE_BIAS_RATE
|
||||
|
||||
# 获取洞口信息
|
||||
self.hole_info = self.environment.get_hole_info()
|
||||
|
||||
@ -5,13 +5,16 @@ Path Optimizer Module
|
||||
路径优化模块，对RRT*生成的路径进行平滑和优化。
|
||||
包括路径简化、平滑处理、速度规划等功能。
|
||||
|
||||
配置驱动：所有参数从config.json读取
|
||||
错误处理：失败立即抛出异常，无后备方案
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from typing import List, Tuple, Optional, Dict, Any
|
||||
|
||||
# 路径优化参数
|
||||
SHORTCUT_ITERATIONS = 50
|
||||
SMOOTHING_FACTOR = 0.5
|
||||
|
||||
|
||||
class PathOptimizer:
|
||||
"""路径优化器"""
|
||||
@ -27,18 +30,16 @@ class PathOptimizer:
|
||||
self.arm_controller = arm_controller
|
||||
self.config_loader = config_loader
|
||||
|
||||
# 从配置读取优化参数
|
||||
config = config_loader.get_full_config()
|
||||
optimization_config = config['path_planning']['optimization']
|
||||
self.shortcut_iterations = optimization_config['shortcut_iterations']
|
||||
self.smoothing_factor = optimization_config['smoothing_factor']
|
||||
# 优化参数（使用文件内常量）
|
||||
self.shortcut_iterations = SHORTCUT_ITERATIONS
|
||||
self.smoothing_factor = SMOOTHING_FACTOR
|
||||
|
||||
# 从配置读取执行参数
|
||||
# 从配置读取跨文件共享参数
|
||||
config = config_loader.get_full_config()
|
||||
execution_config = config['path_planning']['execution']
|
||||
self.velocity_scaling = execution_config['velocity_scaling']
|
||||
self.position_tolerance = execution_config['position_tolerance']
|
||||
|
||||
# 从配置读取碰撞检测参数
|
||||
collision_config = config['path_planning']['collision']
|
||||
self.check_resolution = collision_config['check_resolution']
|
||||
|
||||
|
||||
@ -13,12 +13,16 @@ from urdf_parser_py.urdf import URDF
|
||||
import sys
|
||||
import os
|
||||
|
||||
# 运动学求解参数
|
||||
MAX_ITERATIONS = 1500
|
||||
EPSILON = 1e-05
|
||||
|
||||
|
||||
class KinematicsEngine:
|
||||
"""KDL-based kinematics engine for robot arm calculations"""
|
||||
|
||||
def __init__(self, urdf_path: str, base_position: List[float] = None,
|
||||
base_orientation: List[float] = None, kinematics_config: dict = None):
|
||||
base_orientation: List[float] = None):
|
||||
"""Initialize KDL kinematics engine from URDF file"""
|
||||
self.urdf_path = urdf_path
|
||||
self.base_link: Optional[str] = None
|
||||
@ -30,11 +34,9 @@ class KinematicsEngine:
|
||||
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()
|
||||
|
||||
# Kinematics configuration - 必须提供
|
||||
if kinematics_config is None:
|
||||
raise ValueError("kinematics_config is required")
|
||||
self.max_iterations = kinematics_config['max_iterations']
|
||||
self.epsilon = kinematics_config['epsilon']
|
||||
# 运动学配置（使用文件内常量）
|
||||
self.max_iterations = MAX_ITERATIONS
|
||||
self.epsilon = EPSILON
|
||||
|
||||
# KDL components
|
||||
self.chain: Optional[kdl.Chain] = None
|
||||
@ -396,7 +398,4 @@ def create_kinematics_engine(config_loader) -> KinematicsEngine:
|
||||
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 kinematics config
|
||||
kinematics_config = config_loader.get_full_config()['kinematics']
|
||||
|
||||
return KinematicsEngine(urdf_path, base_position, base_orientation, kinematics_config)
|
||||
return KinematicsEngine(urdf_path, base_position, base_orientation)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user