diff --git a/xr_rm_teleop/test/ik_method_comparison.py b/xr_rm_teleop/test/ik_method_comparison.py index e30732d..1063d69 100644 --- a/xr_rm_teleop/test/ik_method_comparison.py +++ b/xr_rm_teleop/test/ik_method_comparison.py @@ -1,14 +1,20 @@ from __future__ import annotations +import csv +import json import math import time -from dataclasses import dataclass, replace +from dataclasses import asdict, dataclass, replace from pathlib import Path import h5py +import matplotlib import numpy as np import yaml +matplotlib.use("Agg") +import matplotlib.pyplot as plt + from xr_rm_teleop.placo_ik_solver import ( QP_MAX_ITERATIONS, QP_ORIENTATION_TOLERANCE_RAD, @@ -23,6 +29,25 @@ from xr_rm_teleop.single_arm_velocity_teleop import ( _so3_log, ) +METHOD_STYLE = { + "pinv": ("Jacobian 伪逆", "#D55E00", "-"), + "dls": ("DLS", "#0072B2", "--"), + "qp": ("优化 QP", "#009E73", "-."), +} +plt.rcParams.update( + { + "font.family": "sans-serif", + "font.sans-serif": [ + "Noto Sans CJK JP", + "WenQuanYi Micro Hei", + "DejaVu Sans", + ], + "axes.unicode_minus": False, + "figure.dpi": 120, + "savefig.dpi": 300, + } +) + @dataclass(frozen=True) class EpisodeTrajectory: @@ -631,3 +656,347 @@ def evaluate_methods( max_solve_ms=float(np.max(timing_matrix)), ) return results, summaries, selected_damping + + +def _save_figure(fig, output_dir: Path, name: str) -> None: + fig.savefig(output_dir / f"{name}.svg", bbox_inches="tight") + fig.savefig( + output_dir / f"{name}.png", + dpi=300, + bbox_inches="tight", + facecolor="white", + ) + plt.close(fig) + + +def _style_axes(axes) -> None: + for axis in np.asarray(axes).flat: + axis.grid(True, alpha=0.2, linewidth=0.6) + axis.spines["top"].set_visible(False) + axis.spines["right"].set_visible(False) + + +def plot_tracking_error( + results: dict[str, ReplayResult], + output_dir: Path, +) -> None: + fig, axes = plt.subplots( + 2, 1, figsize=(7.2, 5.2), sharex=True, constrained_layout=True + ) + for name, result in results.items(): + label, color, linestyle = METHOD_STYLE[name] + values = ( + result.position_errors_m * 1000.0, + np.degrees(result.orientation_errors_rad), + ) + for axis, series in zip(axes, values): + axis.plot( + result.times_s, + series, + color=color, + linestyle=linestyle, + linewidth=1.15, + label=label, + ) + failed = ~result.success + axis.scatter( + result.times_s[failed], + series[failed], + color=color, + marker="x", + s=10, + linewidths=0.7, + zorder=3, + ) + axes[0].set_ylabel("位置误差 (mm)") + axes[1].set_ylabel("姿态误差 (°)") + axes[1].set_xlabel("时间 (s)") + axes[0].legend(frameon=False, ncol=3, loc="upper right") + _style_axes(axes) + _save_figure(fig, output_dir, "figure_2_11_tracking_error") + + +def plot_joint_constraints( + results: dict[str, ReplayResult], + output_dir: Path, +) -> None: + fig, axes = plt.subplots( + 2, 1, figsize=(7.2, 5.2), sharex=True, constrained_layout=True + ) + for name, result in results.items(): + label, color, linestyle = METHOD_STYLE[name] + speed = np.max(np.abs(np.degrees(result.velocities)), axis=1) + axes[0].plot( + result.times_s, + speed, + color=color, + linestyle=linestyle, + linewidth=1.15, + label=label, + ) + axes[1].plot( + result.times_s, + result.joint_margins, + color=color, + linestyle=linestyle, + linewidth=1.15, + label=label, + ) + axes[0].axhline( + 180.0, + color="#B2182B", + linestyle=":", + linewidth=1.0, + label="速度上限 180°/s", + ) + axes[0].set_ylabel("最大关节速度 (°/s)") + axes[1].set_ylabel("最小归一化关节裕度") + axes[1].set_xlabel("时间 (s)") + axes[0].legend(frameon=False, ncol=2, loc="upper right") + _style_axes(axes) + _save_figure(fig, output_dir, "figure_2_12_joint_constraints") + + +def _annotate_bars(axis, bars) -> None: + for bar in bars: + value = float(bar.get_height()) + axis.annotate( + f"{value:.3g}", + (bar.get_x() + bar.get_width() / 2.0, value), + xytext=(0, 3 if value >= 0.0 else -9), + textcoords="offset points", + ha="center", + va="bottom" if value >= 0.0 else "top", + fontsize=6.5, + ) + + +def plot_summary( + summaries: dict[str, MethodSummary], + output_dir: Path, +) -> None: + names = list(METHOD_STYLE) + labels = [METHOD_STYLE[name][0] for name in names] + colors = [METHOD_STYLE[name][1] for name in names] + values = [ + [summaries[name].position_rmse_m * 1000.0 for name in names], + [np.degrees(summaries[name].orientation_rmse_rad) for name in names], + [summaries[name].max_joint_speed_deg_s for name in names], + [summaries[name].min_joint_margin for name in names], + [summaries[name].success_rate * 100.0 for name in names], + ] + titles = [ + "(a) 位置 RMSE (mm)", + "(b) 姿态 RMSE (°)", + "(c) 最大关节速度 (°/s)", + "(d) 最小归一化关节裕度", + "(f) 求解成功率 (%)", + ] + fig, axes = plt.subplots(2, 3, figsize=(8.2, 5.4), constrained_layout=True) + flat_axes = axes.flat + for axis, data, title in zip( + [flat_axes[0], flat_axes[1], flat_axes[2], flat_axes[3], flat_axes[5]], + values, + titles, + ): + bars = axis.bar( + labels, + data, + color=colors, + edgecolor="black", + linewidth=0.5, + hatch=["//", "\\\\", ".."], + ) + axis.set_title(title, fontsize=9) + axis.tick_params(axis="x", labelrotation=18, labelsize=7) + _annotate_bars(axis, bars) + timing_axis = flat_axes[4] + x = np.arange(len(names)) + width = 0.36 + mean_bars = timing_axis.bar( + x - width / 2.0, + [summaries[name].mean_solve_ms for name in names], + width, + color=colors, + edgecolor="black", + linewidth=0.5, + label="平均", + ) + max_bars = timing_axis.bar( + x + width / 2.0, + [summaries[name].max_solve_ms for name in names], + width, + color=colors, + alpha=0.45, + edgecolor="black", + linewidth=0.5, + label="最大", + ) + timing_axis.set_title("(e) 单周期求解时间 (ms)", fontsize=9) + timing_axis.set_xticks(x, labels, rotation=18) + timing_axis.tick_params(axis="x", labelsize=7) + timing_axis.legend(frameon=False, fontsize=7) + _annotate_bars(timing_axis, mean_bars) + _annotate_bars(timing_axis, max_bars) + _style_axes(axes) + _save_figure(fig, output_dir, "figure_2_13_summary") + + +def _write_samples( + path: Path, + results: dict[str, ReplayResult], +) -> None: + pose_names = [ + "x", "y", "z", "qx", "qy", "qz", "qw", + ] + fields = ( + ["method", "time_s"] + + [f"target_{name}" for name in pose_names] + + [f"actual_{name}" for name in pose_names] + + ["position_error_m", "orientation_error_rad"] + + [f"q{index}" for index in range(1, 8)] + + [f"qd{index}" for index in range(1, 8)] + + ["joint_margin", "solve_ms", "success", "command_limited"] + ) + with path.open("w", encoding="utf-8", newline="") as stream: + writer = csv.writer(stream) + writer.writerow(fields) + for name, result in results.items(): + for index, timestamp in enumerate(result.times_s): + writer.writerow( + [name, timestamp] + + result.target_poses[index].tolist() + + result.actual_poses[index].tolist() + + [ + result.position_errors_m[index], + result.orientation_errors_rad[index], + ] + + result.joints[index].tolist() + + result.velocities[index].tolist() + + [ + result.joint_margins[index], + result.solve_durations_ms[index], + bool(result.success[index]), + bool(result.command_limited[index]), + ] + ) + + +def _method_list_at_extreme( + summaries: dict[str, MethodSummary], + attribute: str, + *, + maximum: bool, +) -> str: + values = {name: getattr(summary, attribute) for name, summary in summaries.items()} + extreme = (max if maximum else min)(values.values()) + return "、".join( + METHOD_STYLE[name][0] + for name, value in values.items() + if math.isclose(value, extreme, rel_tol=1e-9, abs_tol=1e-12) + ) + + +def _write_analysis( + path: Path, + summaries: dict[str, MethodSummary], + selected_damping: float, + source_path: Path, + sample_rate_hz: float, +) -> None: + rows = [] + for name in METHOD_STYLE: + summary = summaries[name] + rows.append( + "| " + + " | ".join( + [ + METHOD_STYLE[name][0], + f"{summary.position_rmse_m:.6f}", + f"{summary.orientation_rmse_rad:.6f}", + f"{summary.max_joint_speed_deg_s:.3f}", + f"{summary.min_joint_margin:.4f}", + f"{summary.mean_solve_ms:.3f} / {summary.max_solve_ms:.3f}", + f"{summary.success_rate * 100.0:.2f}%", + ] + ) + + " |" + ) + position_best = _method_list_at_extreme( + summaries, "position_rmse_m", maximum=False + ) + orientation_best = _method_list_at_extreme( + summaries, "orientation_rmse_rad", maximum=False + ) + success_best = _method_list_at_extreme( + summaries, "success_rate", maximum=True + ) + text = f"""# 2.3.4 三种逆运动学方法对比补充分析 + +本结果是基于真实遥操作目标轨迹的离线运动学对比,不代表真机闭环实验。数据来自 +`{source_path}`,目标位姿以 {sample_rate_hz:.1f} Hz 重采样;三种方法使用同一初始 +关节状态、同一 URDF、相同收敛阈值和共同的输出速度/加速度限制。 + +DLS 扫描的固定阻尼候选为 {", ".join(map(str, DLS_DAMPING_CANDIDATES))},本轨迹选定 +`{selected_damping:g}`。该参数是在当前评价轨迹上选优,不应解释为跨轨迹最优参数。 + +| 方法 | 位置 RMSE (m) | 姿态 RMSE (rad) | 最大关节速度 (°/s) | 最小归一化裕度 | 平均/最大求解时间 (ms) | 成功率 | +| --- | ---: | ---: | ---: | ---: | ---: | ---: | +{chr(10).join(rows)} + +图 2-11 三种逆运动学方法的末端位置与姿态跟踪误差。曲线来自统一时间轴,叉号表示该 +周期数值求解失败并保持上一安全关节状态。 + +图 2-12 三种逆运动学方法的最大关节速度与最小归一化关节安全裕度。红色虚线表示 +180°/s 输出速度上限,裕度越大表示离关节位置边界越远。 + +图 2-13 三种逆运动学方法的综合性能对比,包括误差、关节运动、求解时间和成功率。 + +按本次单轨迹数值比较,位置 RMSE 最低的方法为{position_best},姿态 RMSE 最低的方法为 +{orientation_best},成功率最高的方法为{success_best}。这些结论只描述本次离线复放, +未进行统计显著性检验。 + +当前优化 QP 除六维末端主任务外,还保留项目中的 J3 参考软任务、J4 硬下界与软缓冲, +以及按最小奇异值动态激活的六维可操作度任务;伪逆和 DLS 基线不包含这些附加任务。 +""" + path.write_text(text, encoding="utf-8") + + +def write_outputs( + output_dir: Path, + results: dict[str, ReplayResult], + summaries: dict[str, MethodSummary], + *, + selected_damping: float, + source_path: Path, + git_commit: str, +) -> None: + output_dir = Path(output_dir) + output_dir.mkdir(parents=True, exist_ok=True) + sample_rate_hz = 1.0 / float( + np.median(np.diff(next(iter(results.values())).times_s)) + ) + _write_samples(output_dir / "samples.csv", results) + payload = { + "source_episode": str(source_path), + "git_commit": git_commit, + "sample_rate_hz": sample_rate_hz, + "selected_dls_damping": selected_damping, + "methods": { + name: asdict(summary) for name, summary in summaries.items() + }, + } + (output_dir / "summary.json").write_text( + json.dumps(payload, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + plot_tracking_error(results, output_dir) + plot_joint_constraints(results, output_dir) + plot_summary(summaries, output_dir) + _write_analysis( + output_dir / "analysis_2.3.4.md", + summaries, + selected_damping, + source_path, + sample_rate_hz, + ) diff --git a/xr_rm_teleop/test/test_ik_method_comparison.py b/xr_rm_teleop/test/test_ik_method_comparison.py index b7e6e20..5f4d4d2 100644 --- a/xr_rm_teleop/test/test_ik_method_comparison.py +++ b/xr_rm_teleop/test/test_ik_method_comparison.py @@ -256,3 +256,48 @@ def test_summarize_result_uses_report_metrics() -> None: assert actual.position_rmse_m == pytest.approx(0.0035355339) assert actual.orientation_rmse_rad == pytest.approx(0.0158113883) assert actual.max_joint_speed_deg_s == pytest.approx(180.0) + + +def test_write_outputs_creates_consistent_files(tmp_path: Path) -> None: + result = comparison.ReplayResult( + method="qp", + times_s=np.asarray([0.0, 0.1]), + target_poses=np.tile( + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1) + ), + actual_poses=np.tile( + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1) + ), + joints=np.zeros((2, 7)), + velocities=np.zeros((2, 7)), + position_errors_m=np.asarray([0.001, 0.002]), + orientation_errors_rad=np.asarray([0.001, 0.002]), + joint_margins=np.asarray([0.2, 0.2]), + solve_durations_ms=np.asarray([0.5, 0.6]), + success=np.asarray([True, True]), + command_limited=np.asarray([False, False]), + ) + summary = comparison.summarize_result(result) + + comparison.write_outputs( + tmp_path, + {"pinv": result, "dls": result, "qp": result}, + {"pinv": summary, "dls": summary, "qp": summary}, + selected_damping=0.03, + source_path=Path("episode_0.hdf5"), + git_commit="abc1234", + ) + + expected = { + "samples.csv", + "summary.json", + "figure_2_11_tracking_error.svg", + "figure_2_11_tracking_error.png", + "figure_2_12_joint_constraints.svg", + "figure_2_12_joint_constraints.png", + "figure_2_13_summary.svg", + "figure_2_13_summary.png", + "analysis_2.3.4.md", + } + assert expected == {path.name for path in tmp_path.iterdir()} + assert all((tmp_path / name).stat().st_size > 0 for name in expected)