#!/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_optimizer import PathOptimizer import pybullet as p import pybullet_data import numpy as np import time def test_path_improvement(): """测试路径优化改进""" print("=== 测试路径优化改进 ===") # 初始化仿真（显示GUI） 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_optimizer = PathOptimizer(arm_controller, config_loader) print(f"优化参数设置:") print(f" SHORTCUT_ITERATIONS: {path_optimizer.shortcut_iterations}") print(f" MAX_SHORTCUT_DISTANCE: {path_optimizer.max_shortcut_distance}") print(f" DENSIFICATION_STEP: {path_optimizer.densification_step}") # 创建一个简单的测试路径 current_joints = arm_controller.get_current_joint_positions() target_joints = current_joints.copy() target_joints[0] += 0.5 # 第一个关节转动0.5弧度 target_joints[1] += 0.3 # 第二个关节转动0.3弧度 # 检查起止点距离 start_end_distance = np.linalg.norm(np.array(target_joints) - np.array(current_joints)) print(f"起止点关节空间距离: {start_end_distance:.4f}") print(f"Shortcut距离限制: {path_optimizer.max_shortcut_distance}") # 设置更好的视角 p.resetDebugVisualizerCamera( cameraDistance=4.0, cameraYaw=45, cameraPitch=-30, cameraTargetPosition=[0, 0, 0], physicsClientId=physics_client ) # 创建包含5个中间点的路径 original_path = [] for i in range(6): # 6个点：起点+4个中间点+终点 ratio = i / 5.0 interpolated = np.array(current_joints) * (1 - ratio) + np.array(target_joints) * ratio original_path.append(interpolated.tolist()) print(f"\n原始路径: {len(original_path)} 个点") # 优化路径 optimized_path = path_optimizer.optimize_path(original_path, collision_checker) print(f"优化后路径: {len(optimized_path)} 个点") # 计算轨迹贴合度 print("\n=== 轨迹贴合度分析 ===") # 计算原始路径的笛卡尔轨迹 original_cartesian = [] for config in original_path: pos, _ = arm_controller.forward_kinematics(config) original_cartesian.append(pos) # 计算优化路径的笛卡尔轨迹 optimized_cartesian = [] for config in optimized_path: pos, _ = arm_controller.forward_kinematics(config) optimized_cartesian.append(pos) # 计算关节空间路径长度 def calculate_joint_path_length(path): length = 0 for i in range(len(path) - 1): length += np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) return length # 计算笛卡尔空间路径长度 def calculate_cartesian_path_length(path): length = 0 for i in range(len(path) - 1): length += np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) return length original_joint_length = calculate_joint_path_length(original_path) optimized_joint_length = calculate_joint_path_length(optimized_path) original_cart_length = calculate_cartesian_path_length(original_cartesian) optimized_cart_length = calculate_cartesian_path_length(optimized_cartesian) print(f"原始路径 - 关节空间长度: {original_joint_length:.4f}") print(f"优化路径 - 关节空间长度: {optimized_joint_length:.4f}") print(f"原始路径 - 笛卡尔长度: {original_cart_length:.4f}") print(f"优化路径 - 笛卡尔长度: {optimized_cart_length:.4f}") # 计算最大关节间距 def max_joint_distance(path): max_dist = 0 for i in range(len(path) - 1): dist = np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) max_dist = max(max_dist, dist) return max_dist max_original_dist = max_joint_distance(original_path) max_optimized_dist = max_joint_distance(optimized_path) print(f"原始路径最大关节间距: {max_original_dist:.4f}") print(f"优化路径最大关节间距: {max_optimized_dist:.4f}") # 验证密集化效果 densification_threshold = path_optimizer.densification_step if max_optimized_dist <= densification_threshold: print(f"✅ 密集化成功：最大间距 {max_optimized_dist:.4f} <= 阈值 {densification_threshold}") else: print(f"⚠️ 密集化部分成功：最大间距 {max_optimized_dist:.4f} > 阈值 {densification_threshold}") # 验证shortcut限制效果 print(f"\n=== Shortcut限制验证 ===") shortcut_threshold = path_optimizer.max_shortcut_distance print(f"Shortcut距离限制: {shortcut_threshold}") if len(optimized_path) >= len(original_path) * 0.5: # 保留了至少50%的点 print("✅ Shortcut优化受到限制，保留了足够的中间点") else: print("⚠️ Shortcut优化过度，可能影响轨迹贴合") # 可视化路径对比 print(f"\n=== 路径可视化 ===") print("绘制路径轨迹...") # 绘制原始路径（蓝色） original_line_ids = [] for i in range(len(original_cartesian) - 1): line_id = p.addUserDebugLine( original_cartesian[i], original_cartesian[i + 1], lineColorRGB=[0, 0, 1], # 蓝色 - 原始路径 lineWidth=2, physicsClientId=physics_client ) original_line_ids.append(line_id) # 绘制优化后路径（红色） optimized_line_ids = [] for i in range(len(optimized_cartesian) - 1): line_id = p.addUserDebugLine( optimized_cartesian[i], optimized_cartesian[i + 1], lineColorRGB=[1, 0, 0], # 红色 - 优化路径 lineWidth=3, physicsClientId=physics_client ) optimized_line_ids.append(line_id) # 标记起止点 start_marker = p.addUserDebugLine( original_cartesian[0], [original_cartesian[0][0], original_cartesian[0][1], original_cartesian[0][2] + 0.2], lineColorRGB=[0, 1, 0], # 绿色 - 起点 lineWidth=5, physicsClientId=physics_client ) end_marker = p.addUserDebugLine( original_cartesian[-1], [original_cartesian[-1][0], original_cartesian[-1][1], original_cartesian[-1][2] + 0.2], lineColorRGB=[0, 1, 1], # 青色 - 终点 lineWidth=5, physicsClientId=physics_client ) print("可视化说明:") print(" 蓝色线条 = 原始路径") print(" 红色线条 = 优化后路径") print(" 绿色标记 = 起点") print(" 青色标记 = 终点") # 演示机械臂运动 print(f"\n=== 机械臂运动演示 ===") print("开始执行优化后的路径...") # 设置机械臂到初始位置 arm_controller.set_joint_positions(optimized_path[0]) for _ in range(30): # 等待稳定 p.stepSimulation(physicsClientId=physics_client) time.sleep(0.01) # 逐步执行路径 for i, config in enumerate(optimized_path): print(f"移动到waypoint {i+1}/{len(optimized_path)}") arm_controller.set_joint_positions(config) # 逐步移动，显示中间过程 for _ in range(20): # 每个waypoint停留0.2秒 p.stepSimulation(physicsClientId=physics_client) time.sleep(0.01) print("路径执行完成！") print("\n=== 测试总结 ===") improvements = [] if max_optimized_dist <= densification_threshold: improvements.append("密集化确保关节间距合理") if len(optimized_path) >= len(original_path) * 0.5: improvements.append("Shortcut优化受到限制") if improvements: print("✅ 改进效果:") for improvement in improvements: print(f" - {improvement}") else: print("❌ 改进效果不明显，需要进一步调整参数") print(f"\n仿真将保持开启30秒，请观察路径可视化效果...") print("如需提前关闭，请关闭PyBullet窗口") for i in range(30): time.sleep(1) # 检查是否还连接 try: p.getConnectionInfo(physicsClientId=physics_client) except: print("仿真窗口已关闭") break if i % 5 == 0: print(f"剩余 {30-i} 秒...") except Exception as e: print(f"测试过程中发生错误: {e}") import traceback traceback.print_exc() finally: p.disconnect(physicsClientId=physics_client) if __name__ == "__main__": test_path_improvement()