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()