sladro
d50a493acf
主要改进： - 将三阶段独立执行改为单次完整路径执行 - 分段规划后合并路径，避免误差累积 - 在关键点（物体、A点）自动停留0.5秒 - 调整路径颜色为暗绿色，更柔和 - 降低移动速度3倍，动作更平滑 - 修复硬编码问题，使用常量定义 修复问题： - Stage 2执行失败（6.2cm误差） - 多次规划导致的误差累积 - 理论位置与实际位置不一致 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
230 lines
10 KiB
Python
230 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_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() |