RoboticArmTest/debug_execution.py

#!/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]}")

        # 打印初始状态的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)
        
        # 生成一个简单的测试路径：修改关节角度以产生更大的空间移动
        test_path = []
        for i in range(10):  # 10个路径点（原来5个的两倍）
            test_config = current_joints.copy()
            test_config[0] += i * 0.2  # 每步增加0.2弧度，总共移动1.8弧度（原来0.8的两倍多）
            test_config[1] += i * 0.15  # 每步增加0.15弧度，总共移动1.35弧度
            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())
                
                # 执行多次仿真步进让控制器充分收敛
                for _ in range(10):
                    p.stepSimulation(physicsClientId=physics_client)
                
                # 等待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
                
                # 执行仿真步进让机械臂继续收敛
                for _ in range(10):
                    p.stepSimulation(physicsClientId=physics_client)
                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}")

            # 添加KDL和PyBullet坐标对比
            print(f"\n--- Waypoint {i} 执行完毕坐标对比 ---")

            # KDL正运动学计算
            kdl_result = arm_controller.forward_kinematics(final_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")
            
            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()