64 lines
1.8 KiB
Python
64 lines
1.8 KiB
Python
import os
|
||
import re
|
||
import sys
|
||
import pandas as pd
|
||
import matplotlib.pyplot as plt
|
||
|
||
def main(csv_path: str):
|
||
# 更健壮的读取
|
||
df = pd.read_csv(csv_path, sep=None, engine="python", comment="#")
|
||
if 't' not in df.columns:
|
||
raise RuntimeError("CSV missing 't' column")
|
||
|
||
# ——严格区分列名——
|
||
# 允许列名形如：q0, q1, ... | qd0, qd1, ... | qdd0, qdd1, ...
|
||
def grep(pattern):
|
||
r = re.compile(pattern)
|
||
return [c for c in df.columns if r.fullmatch(c)]
|
||
|
||
q_cols = grep(r"q\d+")
|
||
qd_cols = grep(r"qd\d+")
|
||
qdd_cols = grep(r"qdd\d+")
|
||
|
||
# 如果你的列名不是纯数字后缀（比如 q_joint1），把上面的正则改成更宽松的：
|
||
# q_cols = [c for c in df.columns if c.startswith("q") and not c.startswith(("qd","qdd"))]
|
||
# qd_cols = [c for c in df.columns if c.startswith("qd") and not c.startswith("qdd")]
|
||
# qdd_cols = [c for c in df.columns if c.startswith("qdd")]
|
||
|
||
t = df['t'].values
|
||
|
||
# 位置
|
||
plt.figure()
|
||
for c in q_cols:
|
||
plt.plot(t, df[c].values, label=c)
|
||
plt.xlabel('time [s]')
|
||
plt.ylabel('position [rad]')
|
||
plt.title('Joint Positions')
|
||
plt.legend()
|
||
plt.grid(True)
|
||
|
||
# 速度（只画 qd*）
|
||
plt.figure()
|
||
for c in qd_cols:
|
||
plt.plot(t, df[c].values, label=c)
|
||
plt.xlabel('time [s]')
|
||
plt.ylabel('velocity [rad/s]')
|
||
plt.title('Joint Velocities')
|
||
plt.legend()
|
||
plt.grid(True)
|
||
|
||
# 加速度（只画 qdd*）
|
||
plt.figure()
|
||
for c in qdd_cols:
|
||
plt.plot(t, df[c].values, label=c)
|
||
plt.xlabel('time [s]')
|
||
plt.ylabel('acceleration [rad/s^2]')
|
||
plt.title('Joint Accelerations')
|
||
plt.legend()
|
||
plt.grid(True)
|
||
|
||
plt.show()
|
||
|
||
if __name__ == '__main__':
|
||
main("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv")
|