import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def plot_tool_vectors(filename="tool_vector_log.txt"):
    xs, ys, zs = [], [], []

    with open(filename, "r") as f:
        for line in f:
            if line.strip() == "":
                continue
            # 提取每行的6个值
            parts = line.strip().replace("x:", "").replace("y:", "").replace("z:", "").replace("rx:", "").replace("ry:", "").replace("rz:", "").split(",")
            values = [float(p.strip()) for p in parts]
            xs.append(values[0])
            ys.append(values[1])
            zs.append(values[2])

    # 绘制 3D 轨迹
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(xs, ys, zs, marker='o')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.set_title('Tool Vector Position Trajectory')
    plt.show()