#!/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()