27 lines
887 B
Python
27 lines
887 B
Python
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()
|