✨ feat(*): 添加触发样本偏移与实发轨迹分析导出
* 为 RobotConfig 增加 trigger_sample_index_offset_cycles 配置 * 让 DO 事件携带示教点关节角并按最接近 sample 绑定触发 * 调整运行时 IO 地址位掩码映射并补充 ShotEvents 导出 * 新增 2026042802-1 抓包分析脚本、数据产物与结论文档 * 补齐配置兼容、规划绑定和运行时触发相关测试
This commit is contained in:
137
analysis/build_trigger_offset_compare_6_7_8.py
Normal file
137
analysis/build_trigger_offset_compare_6_7_8.py
Normal file
@@ -0,0 +1,137 @@
|
||||
#!/usr/bin/env python3
|
||||
"""生成 2026042802-1 抓包中触发偏移 6/7/8 周期的对照表。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
|
||||
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
|
||||
ACTUAL_SEND_CSV = OUTPUT_DIR / "2026042802-1_j519_actual_send_all_with_io.csv"
|
||||
TRIGGER_CSV = OUTPUT_DIR / "2026042802-1_j519_trigger_frames.csv"
|
||||
OUTPUT_CSV = OUTPUT_DIR / "2026042802-1_trigger_offset_6_7_8_compare.csv"
|
||||
OUTPUT_JSON = OUTPUT_DIR / "2026042802-1_trigger_offset_6_7_8_summary.json"
|
||||
|
||||
|
||||
def load_rows(path: Path) -> list[dict]:
|
||||
with path.open(encoding="utf-8") as handle:
|
||||
return list(csv.DictReader(handle))
|
||||
|
||||
|
||||
def to_float_list(record: dict, prefix: str = "j") -> list[float]:
|
||||
return [float(record[f"{prefix}{index}_deg"]) for index in range(1, 7)]
|
||||
|
||||
|
||||
def compute_diff_metrics(actual_deg: list[float], teach_deg: list[float]) -> tuple[list[float], float, float, str]:
|
||||
diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
|
||||
abs_diffs = [abs(value) for value in diffs]
|
||||
max_error = max(abs_diffs)
|
||||
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
|
||||
max_axis = f"J{abs_diffs.index(max_error) + 1}"
|
||||
return diffs, max_error, rms_error, max_axis
|
||||
|
||||
|
||||
def main() -> None:
|
||||
actual_rows = load_rows(ACTUAL_SEND_CSV)
|
||||
trigger_rows = load_rows(TRIGGER_CSV)
|
||||
config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))["flying_shots"]["UTTC_MS11"]
|
||||
trigger_waypoint_indices = [index for index, flag in enumerate(config["shot_flags"]) if flag]
|
||||
|
||||
actual_index_by_frame = {int(row["frame_number"]): row for row in actual_rows}
|
||||
actual_order_by_frame = {int(row["frame_number"]): idx for idx, row in enumerate(actual_rows)}
|
||||
|
||||
output_rows: list[dict] = []
|
||||
offset_win_counts = {6: 0, 7: 0, 8: 0}
|
||||
|
||||
for trigger_no, (trigger_row, waypoint_index) in enumerate(zip(trigger_rows, trigger_waypoint_indices), start=1):
|
||||
trigger_frame = int(trigger_row["frame_number"])
|
||||
trigger_order = actual_order_by_frame[trigger_frame]
|
||||
teach_deg = [math.degrees(value) for value in config["traj_waypoints"][waypoint_index]]
|
||||
|
||||
window_start = max(0, trigger_order - 20)
|
||||
window_end = min(len(actual_rows) - 1, trigger_order + 20)
|
||||
best_order = trigger_order
|
||||
best_rms = float("inf")
|
||||
best_max = float("inf")
|
||||
|
||||
for candidate_order in range(window_start, window_end + 1):
|
||||
candidate = actual_rows[candidate_order]
|
||||
_, max_error, rms_error, _ = compute_diff_metrics(to_float_list(candidate), teach_deg)
|
||||
score = (rms_error, max_error, abs(candidate_order - trigger_order))
|
||||
if (best_rms, best_max, abs(best_order - trigger_order)) > score:
|
||||
best_order = candidate_order
|
||||
best_rms = rms_error
|
||||
best_max = max_error
|
||||
|
||||
row = {
|
||||
"trigger_no": trigger_no,
|
||||
"waypoint_index": waypoint_index,
|
||||
"best_sample_order": best_order,
|
||||
"best_frame_number": int(actual_rows[best_order]["frame_number"]),
|
||||
"best_sequence": int(actual_rows[best_order]["sequence"]),
|
||||
"best_time_relative_s": float(actual_rows[best_order]["time_relative_s"]),
|
||||
}
|
||||
|
||||
for joint_index in range(6):
|
||||
row[f"teach_j{joint_index + 1}_deg"] = teach_deg[joint_index]
|
||||
|
||||
for offset in (6, 7, 8):
|
||||
target_order = min(len(actual_rows) - 1, best_order + offset)
|
||||
target = actual_rows[target_order]
|
||||
actual_deg = to_float_list(target)
|
||||
diffs, max_error, rms_error, max_axis = compute_diff_metrics(actual_deg, teach_deg)
|
||||
|
||||
row[f"offset_{offset}_frame_number"] = int(target["frame_number"])
|
||||
row[f"offset_{offset}_sequence"] = int(target["sequence"])
|
||||
row[f"offset_{offset}_time_relative_s"] = float(target["time_relative_s"])
|
||||
row[f"offset_{offset}_max_error_axis"] = max_axis
|
||||
row[f"offset_{offset}_max_error_deg"] = max_error
|
||||
row[f"offset_{offset}_rms_error_deg"] = rms_error
|
||||
row[f"offset_{offset}_delta_from_best_ms"] = (
|
||||
float(target["time_relative_s"]) - float(actual_rows[best_order]["time_relative_s"])
|
||||
) * 1000.0
|
||||
for joint_index in range(6):
|
||||
joint_no = joint_index + 1
|
||||
row[f"offset_{offset}_j{joint_no}_actual_deg"] = actual_deg[joint_index]
|
||||
row[f"offset_{offset}_diff_j{joint_no}_deg"] = diffs[joint_index]
|
||||
|
||||
best_offset = min(
|
||||
(6, 7, 8),
|
||||
key=lambda offset: (
|
||||
row[f"offset_{offset}_rms_error_deg"],
|
||||
row[f"offset_{offset}_max_error_deg"],
|
||||
),
|
||||
)
|
||||
row["best_of_6_7_8_offset"] = best_offset
|
||||
offset_win_counts[best_offset] += 1
|
||||
output_rows.append(row)
|
||||
|
||||
OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
|
||||
with OUTPUT_CSV.open("w", newline="", encoding="utf-8") as handle:
|
||||
writer = csv.DictWriter(handle, fieldnames=list(output_rows[0].keys()))
|
||||
writer.writeheader()
|
||||
writer.writerows(output_rows)
|
||||
|
||||
summary = {
|
||||
"rows": len(output_rows),
|
||||
"best_offset_win_counts": offset_win_counts,
|
||||
"average_max_error_deg": {
|
||||
str(offset): sum(row[f"offset_{offset}_max_error_deg"] for row in output_rows) / len(output_rows)
|
||||
for offset in (6, 7, 8)
|
||||
},
|
||||
"average_rms_error_deg": {
|
||||
str(offset): sum(row[f"offset_{offset}_rms_error_deg"] for row in output_rows) / len(output_rows)
|
||||
for offset in (6, 7, 8)
|
||||
},
|
||||
}
|
||||
OUTPUT_JSON.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user