72 lines
2.0 KiB
Python
72 lines
2.0 KiB
Python
import numpy as np
|
||
import matplotlib.pyplot as plt
|
||
|
||
CSV_PATH = "/home/lgv/cmvr/0-workspace/cmvr-es/data/ik_movel_scurve.csv" # 改成你的路径，比如 /home/lgv/.../ik_movel_scurve.csv
|
||
|
||
def load_csv(path: str):
|
||
# 期望列名：t,q1,q2,q3,q4,q5,q6,q7
|
||
data = np.genfromtxt(path, delimiter=",", names=True)
|
||
t = data["t"].astype(float)
|
||
q = np.vstack([data[f"q{i}"] for i in range(1, 8)]).T.astype(float) # (N,7)
|
||
return t, q
|
||
|
||
def diff_over_time(x: np.ndarray, t: np.ndarray):
|
||
# x: (N,7), t: (N,)
|
||
dt = np.diff(t)
|
||
# 防止 dt=0
|
||
dt = np.where(dt <= 1e-12, 1e-12, dt)
|
||
dx = np.diff(x, axis=0)
|
||
return dx / dt[:, None] # (N-1,7)
|
||
|
||
def main():
|
||
t, q = load_csv(CSV_PATH)
|
||
if len(t) < 3:
|
||
raise RuntimeError("CSV too short (need >= 3 samples).")
|
||
|
||
qd = diff_over_time(q, t) # (N-1,7)
|
||
t_qd = (t[1:] + t[:-1]) * 0.5 # 中点时间
|
||
qdd = diff_over_time(qd, t_qd) # (N-2,7)
|
||
t_qdd = (t_qd[1:] + t_qd[:-1]) * 0.5
|
||
|
||
# 打印统计
|
||
print("=== Stats from CSV ===")
|
||
for j in range(7):
|
||
max_qd = np.max(np.abs(qd[:, j]))
|
||
max_qdd = np.max(np.abs(qdd[:, j]))
|
||
print(f"joint {j+1}: max|qdot|={max_qd:.6f} rad/s, max|qddot|={max_qdd:.6f} rad/s^2")
|
||
|
||
# 画 q(t)
|
||
plt.figure()
|
||
for j in range(7):
|
||
plt.plot(t, q[:, j], label=f"q{j+1}")
|
||
plt.xlabel("t [s]")
|
||
plt.ylabel("q [rad]")
|
||
plt.title("Joint position q(t)")
|
||
plt.grid(True)
|
||
plt.legend()
|
||
|
||
# 画 qdot(t)
|
||
plt.figure()
|
||
for j in range(7):
|
||
plt.plot(t_qd, qd[:, j], label=f"qdot{j+1}")
|
||
plt.xlabel("t [s]")
|
||
plt.ylabel("qdot [rad/s]")
|
||
plt.title("Joint velocity qdot(t) (finite difference)")
|
||
plt.grid(True)
|
||
plt.legend()
|
||
|
||
# 画 qddot(t)
|
||
plt.figure()
|
||
for j in range(7):
|
||
plt.plot(t_qdd, qdd[:, j], label=f"qddot{j+1}")
|
||
plt.xlabel("t [s]")
|
||
plt.ylabel("qddot [rad/s^2]")
|
||
plt.title("Joint acceleration qddot(t) (finite difference)")
|
||
plt.grid(True)
|
||
plt.legend()
|
||
|
||
plt.show()
|
||
|
||
if __name__ == "__main__":
|
||
main()
|