#!/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("=== 路径执行调试分析（使用RRT*路径规划） ===") # 初始化仿真 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) # AIRRTStarPlanner需要environment参数，不是collision_checker planner = AIRRTStarPlanner(arm_controller, environment, config_loader) print("=== 步骤1: 设置自定义起点和终点 ===") # 获取当前位置作为起点 current_joints = arm_controller.get_current_joint_positions() print(f"实际起始关节配置: {[f'{x:.3f}' for x in current_joints]}") # 打印初始状态的KDL和PyBullet坐标对比 print("\n=== 初始状态坐标对比 ===") # KDL正运动学计算 kdl_result = arm_controller.forward_kinematics(current_joints) kdl_position = kdl_result[0] if kdl_result else [0, 0, 0] print(f"KDL计算末端坐标: [{kdl_position[0]:.4f}, {kdl_position[1]:.4f}, {kdl_position[2]:.4f}]") # PyBullet仿真中的实际末端坐标 end_link_state = p.getLinkState(arm_controller.robot_loader.robot_id, arm_controller.robot_loader.end_effector_index, physicsClientId=physics_client) pybullet_position = list(end_link_state[0]) print(f"PyBullet末端坐标: [{pybullet_position[0]:.4f}, {pybullet_position[1]:.4f}, {pybullet_position[2]:.4f}]") # 计算误差 kdl_pb_error = np.linalg.norm(np.array(kdl_position) - np.array(pybullet_position)) print(f"KDL与PyBullet误差: {kdl_pb_error:.6f}m") print("=" * 50) # 测试第二段的起始和目标位置 # 物体位置（第一段终点） object_position = [0.6, 0.5, 0.5] # A点位置（第二段终点） point_a = [0.5, 0.5, 0.8] print(f"\n测试Stage 2路径：") print(f"起点（物体位置）: {object_position}") print(f"终点（A点位置）: {point_a}") # 先移动到物体位置作为起点 object_joints = arm_controller.inverse_kinematics(object_position, None, current_joints) if object_joints is None: print("❌ 无法计算物体位置的逆运动学解") return print(f"物体位置关节配置: {[f'{x:.3f}' for x in object_joints]}") # 移动到物体位置 arm_controller.set_joint_positions(object_joints) for _ in range(100): # 等待稳定 p.stepSimulation(physicsClientId=physics_client) time.sleep(0.5) # 获取实际到达的位置 actual_start_joints = arm_controller.get_current_joint_positions() print(f"实际起始关节配置: {[f'{x:.3f}' for x in actual_start_joints]}") # 设置目标为A点 target_position = point_a # 使用逆运动学计算目标关节配置（用实际位置作为seed） target_joints = arm_controller.inverse_kinematics(target_position, None, actual_start_joints) if target_joints is None: print("❌ 无法计算目标位置的逆运动学解") return print(f"目标关节配置: {[f'{x:.3f}' for x in target_joints]}") print("\n=== 步骤2: 使用RRT*规划路径 ===") print(f"规划起点（实际关节）: {[f'{x:.3f}' for x in actual_start_joints]}") print(f"规划终点（目标关节）: {[f'{x:.3f}' for x in target_joints]}") # 规划路径（从实际位置到目标） test_path = planner.plan(actual_start_joints, target_joints) if test_path is None: print("❌ 路径规划失败") return print(f"规划成功！路径包含 {len(test_path)} 个waypoint") print("\n=== 步骤3: 分析路径的理论末端位置 ===") theoretical_positions = [] for i, config in enumerate(test_path): pos, ori = arm_controller.forward_kinematics(config) theoretical_positions.append(pos) if i < 5 or i >= len(test_path) - 5: # 只打印前5个和后5个 print(f"Waypoint {i}: 末端位置{[f'{x:.3f}' for x in pos]}") print("\n=== 步骤4: 可视化理论路径 ===") # 绘制理论路径线条 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=== 步骤5: 使用PathExecutor执行路径（与main_window.py相同） ===") actual_positions = [] # 重置机械臂到实际起始位置（物体位置） arm_controller.set_joint_positions(actual_start_joints) for _ in range(50): # 等待稳定 p.stepSimulation(physicsClientId=physics_client) # 定义一个函数来打印坐标对比（在路径执行过程中调用） def print_coordinate_comparison(stage_name, joints): print(f"\n--- {stage_name} 坐标对比 ---") # KDL正运动学计算 kdl_result = arm_controller.forward_kinematics(joints) kdl_pos = kdl_result[0] if kdl_result else [0, 0, 0] print(f"KDL计算末端坐标: [{kdl_pos[0]:.4f}, {kdl_pos[1]:.4f}, {kdl_pos[2]:.4f}]") # PyBullet仿真中的实际末端坐标 end_link_state = p.getLinkState(arm_controller.robot_loader.robot_id, arm_controller.robot_loader.end_effector_index, physicsClientId=physics_client) pb_pos = list(end_link_state[0]) print(f"PyBullet末端坐标: [{pb_pos[0]:.4f}, {pb_pos[1]:.4f}, {pb_pos[2]:.4f}]") # get_current_pose的返回值（调用arm_controller的方法） current_pos, _ = arm_controller.get_current_pose() print(f"get_current_pose返回: [{current_pos[0]:.4f}, {current_pos[1]:.4f}, {current_pos[2]:.4f}]") # 计算误差 kdl_pb_error = np.linalg.norm(np.array(kdl_pos) - np.array(pb_pos)) get_pb_error = np.linalg.norm(np.array(current_pos) - np.array(pb_pos)) print(f"KDL与PyBullet误差: {kdl_pb_error:.6f}m") print(f"get_current_pose与PyBullet误差: {get_pb_error:.6f}m") return pb_pos # 执行路径前的坐标对比 print_coordinate_comparison("执行路径前", actual_start_joints) # 使用PathExecutor执行路径（与main_window.py相同） print("\n开始使用PathExecutor执行路径...") try: success = path_executor.execute_path(test_path) if success: print("✅ 路径执行成功") else: print("❌ 路径执行失败") except Exception as e: print(f"❌ 路径执行出错: {e}") # 执行完成后的坐标对比 final_joints = arm_controller.get_current_joint_positions() actual_pos = print_coordinate_comparison("路径执行完成后", final_joints) actual_positions.append(actual_pos) # 对比目标位置和实际位置 print(f"\n=== 最终结果对比 ===") print(f"目标位置: [{target_position[0]:.4f}, {target_position[1]:.4f}, {target_position[2]:.4f}]") print(f"实际到达位置(PyBullet): [{actual_pos[0]:.4f}, {actual_pos[1]:.4f}, {actual_pos[2]:.4f}]") final_error = np.linalg.norm(np.array(target_position) - np.array(actual_pos)) print(f"最终位置误差: {final_error:.6f}m") if final_error < 0.02: print("✅ 路径执行精度良好") else: print("❌ 路径执行存在显著误差") # 绘制目标点 p.addUserDebugText("Target", target_position, textColorRGB=[1, 0, 0], textSize=1.5, physicsClientId=physics_client) p.addUserDebugPoints([target_position], pointColorsRGB=[[1, 0, 0]], pointSize=10, physicsClientId=physics_client) # 绘制实际到达点 p.addUserDebugText("Actual", actual_pos, textColorRGB=[0, 0, 1], textSize=1.5, physicsClientId=physics_client) p.addUserDebugPoints([actual_pos], pointColorsRGB=[[0, 0, 1]], pointSize=10, physicsClientId=physics_client) print("\n=== 重要发现 ===") print("如果KDL与PyBullet误差很大，说明存在以下可能问题：") print("1. 关节顺序映射错误（kdl_to_pb或pb_to_kdl转换有误）") print("2. get_current_pose方法返回的是KDL计算值而非PyBullet实际值") print("3. 路径执行器依赖的是get_current_pose，导致误差累积") 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()