sladro
a9db87d81d
将单文件使用的配置改为文件内常量定义： - 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>
230 lines
9.8 KiB
Python
230 lines
9.8 KiB
Python
#!/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() |