sladro
0e1652d621
核心改进： - 限制shortcut优化距离（0.3→0.15），减少迭代次数（50→5） - 新增路径密集化功能，确保关节间距≤0.05弧度 - 在_simplify_path中添加距离限制，防止过度优化 - 添加_densify_path方法保证轨迹安全性 技术成果： - 路径点从6个增加到24个，最大关节间距从0.1166降至0.0254 - 确保机械臂末端严格沿规划路径移动，解决轨迹不可控问题 - 支持不同自由度机械臂，遵循配置驱动原则 测试验证： - 新增test_path_improvement.py演示改进效果 - GUI可视化对比原始路径和优化路径 - 实时机械臂运动验证轨迹贴合度 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
258 lines
10 KiB
Python
258 lines
10 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_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() |