YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
YikaiFu-cart
2026-08-24 17:55:44 +08:00
parent b0f4fafd54
commit 6df791b00f
2 changed files with 415 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/ik_method_comparison.py
+370 -1
View File
@@ -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,
)
xr_rm_teleop/test/test_ik_method_comparison.py
+45
View File
@@ -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)