74 lines
2.3 KiB
Python
74 lines
2.3 KiB
Python
import pandas as pd
|
||
import numpy as np
|
||
import matplotlib.pyplot as plt
|
||
|
||
CSV = '/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv'
|
||
df = pd.read_csv(CSV)
|
||
|
||
# ---- 曲线图：q1~q7 (+ 可选 psi) vs index ----
|
||
joint_cols_expect = ['q1','q2','q3','q4','q5','q6','q7']
|
||
joint_cols = [c for c in joint_cols_expect if c in df.columns]
|
||
assert len(joint_cols) == 7, f"CSV 缺少关节列，期望 {joint_cols_expect}，实际 {list(df.columns)}"
|
||
|
||
x = np.arange(len(df))
|
||
plt.figure(figsize=(12, 5))
|
||
for c in joint_cols:
|
||
plt.plot(x, df[c].to_numpy(), label=c, linewidth=1.2)
|
||
|
||
if 'psi' in df.columns:
|
||
plt.plot(x, df['psi'].to_numpy(), '--', label='psi', linewidth=1.2)
|
||
|
||
plt.xlabel('index')
|
||
plt.ylabel('angle (rad)')
|
||
plt.title('q1..q7 (and psi) vs index')
|
||
plt.grid(True, alpha=0.35)
|
||
plt.legend(ncol=4, fontsize=9)
|
||
plt.tight_layout()
|
||
plt.show()
|
||
|
||
# ---- 柱状图：最近限位距离（选“最危险帧”） ----
|
||
limits = np.array([
|
||
[-0.26, 1.57],
|
||
[-0.78, 1.57],
|
||
[-np.pi, np.pi],
|
||
[ 0.00, 2.05],
|
||
[-3.00, 3.00],
|
||
[-2.00, 2.00],
|
||
[-0.57, 1.57],
|
||
])
|
||
|
||
Q = df[joint_cols].to_numpy() # [N,7]
|
||
lo = limits[:, 0][None, :] # [1,7]
|
||
hi = limits[:, 1][None, :]
|
||
|
||
dist_low = Q - lo # 到下限的距离
|
||
dist_high = hi - Q # 到上限的距离
|
||
nearest = np.minimum(dist_low, dist_high) # 最近限位（可为负，负值=超限）
|
||
|
||
# 找“最危险”的 index（全关节最小裕度最小）
|
||
min_margin_per_row = nearest.min(axis=1) # 每帧的最小关节裕度
|
||
worst_idx = int(np.argmin(min_margin_per_row))
|
||
vals = nearest[worst_idx, :]
|
||
|
||
psi_text = f", psi={df['psi'].iloc[worst_idx]:.4f}" if 'psi' in df.columns else ""
|
||
|
||
plt.figure(figsize=(9, 4.5))
|
||
plt.bar(joint_cols, vals)
|
||
plt.axhline(0.0, linewidth=1, color='k')
|
||
plt.ylabel('Nearest distance to limit (rad)')
|
||
plt.title(f'Per-joint margin at worst frame (index={worst_idx}{psi_text})')
|
||
plt.grid(True, axis='y', alpha=0.35)
|
||
plt.tight_layout()
|
||
plt.show()
|
||
|
||
# ---- 可选：整段最小裕度曲线（帮助定位危险段）----
|
||
plt.figure(figsize=(12, 3.2))
|
||
plt.plot(min_margin_per_row, linewidth=1.2)
|
||
plt.axhline(0.0, linewidth=1, color='k')
|
||
plt.xlabel('index')
|
||
plt.ylabel('min margin (rad)')
|
||
plt.title('Minimum per-frame joint margin over sweep (rad)')
|
||
plt.grid(True, alpha=0.35)
|
||
plt.tight_layout()
|
||
plt.show()
|