feat: 生成逆运动学论文对比图

2026-08-24 17:55:44 +08:00
parent b0f4fafd54
commit 6df791b00f
2 changed files with 415 additions and 1 deletions
@@ -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,
+    )