51 lines
1.2 KiB
Python
51 lines
1.2 KiB
Python
|
import numpy as np
|
||
|
|
import matplotlib.pyplot as plt
|
||
|
|
|
||
|
|
CSV = "/home/lgv/cmvr/0-workspace/cmvr-es/data/ik_movel_scurve_tcp.csv" # 改成你的路径
|
||
|
|
|
||
|
|
data = np.genfromtxt(CSV, delimiter=",", names=True)
|
||
|
|
|
||
|
|
t = data["t"]
|
||
|
|
px, py, pz = data["px"], data["py"], data["pz"]
|
||
|
|
vx, vy, vz = data["vx"], data["vy"], data["vz"]
|
||
|
|
ax, ay, az = data["ax"], data["ay"], data["az"]
|
||
|
|
speed = data["speed"]
|
||
|
|
acc = data["acc"]
|
||
|
|
|
||
|
|
# 位置
|
||
|
|
plt.figure()
|
||
|
|
plt.plot(t, px, label="px")
|
||
|
|
plt.plot(t, py, label="py")
|
||
|
|
plt.plot(t, pz, label="pz")
|
||
|
|
plt.xlabel("t [s]")
|
||
|
|
plt.ylabel("TCP position [m]")
|
||
|
|
plt.title("TCP position (base frame)")
|
||
|
|
plt.grid(True)
|
||
|
|
plt.legend()
|
||
|
|
|
||
|
|
# 速度分量 + 模长
|
||
|
|
plt.figure()
|
||
|
|
plt.plot(t, vx, label="vx")
|
||
|
|
plt.plot(t, vy, label="vy")
|
||
|
|
plt.plot(t, vz, label="vz")
|
||
|
|
plt.plot(t, speed, label="|v|")
|
||
|
|
plt.xlabel("t [s]")
|
||
|
|
plt.ylabel("TCP velocity [m/s]")
|
||
|
|
plt.title("TCP velocity (finite-diff)")
|
||
|
|
plt.grid(True)
|
||
|
|
plt.legend()
|
||
|
|
|
||
|
|
# 加速度分量 + 模长
|
||
|
|
plt.figure()
|
||
|
|
plt.plot(t, ax, label="ax")
|
||
|
|
plt.plot(t, ay, label="ay")
|
||
|
|
plt.plot(t, az, label="az")
|
||
|
|
plt.plot(t, acc, label="|a|")
|
||
|
|
plt.xlabel("t [s]")
|
||
|
|
plt.ylabel("TCP accel [m/s^2]")
|
||
|
|
plt.title("TCP acceleration (finite-diff)")
|
||
|
|
plt.grid(True)
|
||
|
|
plt.legend()
|
||
|
|
|
||
|
|
plt.show()
|