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:52:01 +08:00
parent 398a50b0b3
commit b0f4fafd54
2 changed files with 215 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/ik_method_comparison.py
+163 -10
View File
@@ -2,11 +2,12 @@ from __future__ import annotations
import math
import time
from dataclasses import dataclass
from dataclasses import dataclass, replace
from pathlib import Path
import h5py
import numpy as np
import yaml
from xr_rm_teleop.placo_ik_solver import (
QP_MAX_ITERATIONS,
@@ -39,6 +40,12 @@ class MethodSummary:
position_rmse_m: float
orientation_rmse_rad: float
max_joint_speed_deg_s: float
min_joint_margin: float = 0.0
mean_solve_ms: float = 0.0
max_solve_ms: float = 0.0
failure_count: int = 0
longest_failure_streak: int = 0
command_limited_count: int = 0
@dataclass(frozen=True)
@@ -197,6 +204,25 @@ def load_episode(path: Path) -> EpisodeTrajectory:
)
def load_right_config(path: Path) -> dict[str, float]:
with Path(path).open("r", encoding="utf-8") as stream:
document = yaml.safe_load(stream)
parameters = document["single_arm_velocity_teleop"]["ros__parameters"]
names = (
"qp_j3_reference_deg",
"qp_j3_weight",
"qp_j4_min_deg",
"qp_j4_warn_deg",
"qp_j4_weight",
"qp_manipulability_sigma_stop",
"qp_manipulability_sigma_warn",
"qp_manipulability_weight",
"joint_max_speed",
"joint_max_acc",
)
return {name: float(parameters[name]) for name in names}
def _rotation_z(angle: float) -> np.ndarray:
cosine, sine = math.cos(angle), math.sin(angle)
return np.asarray(
@@ -247,6 +273,40 @@ def choose_dls_damping(candidates: list[MethodSummary]) -> float:
return float(selected.damping)
def _longest_failure_streak(success: np.ndarray) -> int:
longest = current = 0
for value in np.asarray(success, dtype=bool):
current = 0 if value else current + 1
longest = max(longest, current)
return longest
def summarize_result(
result: ReplayResult,
damping: float | None = None,
) -> MethodSummary:
return MethodSummary(
method=result.method,
damping=damping,
success_rate=float(np.mean(result.success)),
position_rmse_m=float(
np.sqrt(np.mean(result.position_errors_m**2))
),
orientation_rmse_rad=float(
np.sqrt(np.mean(result.orientation_errors_rad**2))
),
max_joint_speed_deg_s=float(
np.max(np.abs(np.degrees(result.velocities)))
),
min_joint_margin=float(np.min(result.joint_margins)),
mean_solve_ms=float(np.mean(result.solve_durations_ms)),
max_solve_ms=float(np.max(result.solve_durations_ms)),
failure_count=int(np.count_nonzero(~result.success)),
longest_failure_streak=_longest_failure_streak(result.success),
command_limited_count=int(np.count_nonzero(result.command_limited)),
)
def limit_joint_command(
*,
target: np.ndarray,
@@ -385,20 +445,28 @@ class QpSolverAdapter:
return self._solver.solve(target)
def make_qp_solver(urdf_path: Path, dt: float) -> QpSolverAdapter:
def make_qp_solver(
urdf_path: Path,
dt: float,
config: dict[str, float],
) -> QpSolverAdapter:
return QpSolverAdapter(
PlacoIkSolver(
str(urdf_path),
dt,
"right",
j3_reference_deg=-89.57,
j3_weight=1e-4,
j4_min_deg=10.0,
j4_warn_deg=25.0,
j4_weight=1e-4,
manipulability_sigma_stop=0.01,
manipulability_sigma_warn=0.04,
manipulability_weight=1e-4,
j3_reference_deg=config["qp_j3_reference_deg"],
j3_weight=config["qp_j3_weight"],
j4_min_deg=config["qp_j4_min_deg"],
j4_warn_deg=config["qp_j4_warn_deg"],
j4_weight=config["qp_j4_weight"],
manipulability_sigma_stop=config[
"qp_manipulability_sigma_stop"
],
manipulability_sigma_warn=config[
"qp_manipulability_sigma_warn"
],
manipulability_weight=config["qp_manipulability_weight"],
)
)
@@ -478,3 +546,88 @@ def run_replay(
success=success,
command_limited=command_limited,
)
DLS_DAMPING_CANDIDATES = (0.001, 0.003, 0.01, 0.03, 0.1, 0.3)
def evaluate_methods(
trajectory: EpisodeTrajectory,
urdf_path: Path,
config: dict[str, float],
timing_repeats: int,
) -> tuple[dict[str, ReplayResult], dict[str, MethodSummary], float]:
if timing_repeats < 1:
raise ValueError("timing_repeats must be positive")
dt = float(np.median(np.diff(trajectory.times_s)))
speed = math.radians(config["joint_max_speed"])
acceleration = math.radians(config["joint_max_acc"])
damping_results = []
for damping in DLS_DAMPING_CANDIDATES:
replay = run_replay(
"dls",
DifferentialIkSolver(urdf_path, dt, "dls", damping),
trajectory,
max_speed=speed,
max_acceleration=acceleration,
measure_time=False,
)
damping_results.append(summarize_result(replay, damping))
selected_damping = choose_dls_damping(damping_results)
factories = {
"pinv": lambda: DifferentialIkSolver(urdf_path, dt, "pinv"),
"dls": lambda: DifferentialIkSolver(
urdf_path, dt, "dls", selected_damping
),
"qp": lambda: make_qp_solver(urdf_path, dt, config),
}
results = {}
summaries = {}
for name, factory in factories.items():
run_replay(
name,
factory(),
trajectory,
max_speed=speed,
max_acceleration=acceleration,
)
result = run_replay(
name,
factory(),
trajectory,
max_speed=speed,
max_acceleration=acceleration,
)
timed = []
for _ in range(timing_repeats):
repeated = run_replay(
name,
factory(),
trajectory,
max_speed=speed,
max_acceleration=acceleration,
)
if not np.allclose(
repeated.joints,
result.joints,
atol=1e-10,
rtol=0.0,
):
raise RuntimeError(f"{name} replay is not deterministic")
timed.append(repeated.solve_durations_ms)
timing_matrix = np.stack(timed)
result = replace(
result,
solve_durations_ms=np.mean(timing_matrix, axis=0),
)
results[name] = result
summaries[name] = replace(
summarize_result(
result,
selected_damping if name == "dls" else None,
),
mean_solve_ms=float(np.mean(timing_matrix)),
max_solve_ms=float(np.max(timing_matrix)),
)
return results, summaries, selected_damping
xr_rm_teleop/test/test_ik_method_comparison.py
+52 -1
View File
@@ -188,13 +188,23 @@ def test_replay_holds_previous_state_on_solver_failure() -> None:
def test_real_urdf_solvers_return_finite_safe_outputs() -> None:
pytest.importorskip("placo")
urdf = TEST_DIR.parent / "models" / "dual_rm75" / "Dual_arm.urdf"
config_path = (
TEST_DIR.parents[1]
/ "xr_rm_bringup"
/ "config"
/ "right_arm_rm75.yaml"
)
joints = np.radians(
[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
)
solvers = [
comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "pinv"),
comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "dls", 0.03),
comparison.make_qp_solver(urdf, 1.0 / 90.0),
comparison.make_qp_solver(
urdf,
1.0 / 90.0,
comparison.load_right_config(config_path),
),
]
for solver in solvers:
target = solver.update_joint_state(joints.tolist())
@@ -205,3 +215,44 @@ def test_real_urdf_solvers_return_finite_safe_outputs() -> None:
assert np.isfinite(result).all()
assert np.all(result >= solver.joint_limits[:, 0] - 1e-9)
assert np.all(result <= solver.joint_limits[:, 1] + 1e-9)
def test_load_right_config_returns_qp_and_command_limits() -> None:
config = (
TEST_DIR.parents[1]
/ "xr_rm_bringup"
/ "config"
/ "right_arm_rm75.yaml"
)
actual = comparison.load_right_config(config)
assert actual["qp_j3_reference_deg"] == pytest.approx(-89.57)
assert actual["qp_j4_min_deg"] == pytest.approx(10.0)
assert actual["qp_manipulability_weight"] == pytest.approx(1e-4)
assert actual["joint_max_speed"] == pytest.approx(180.0)
assert actual["joint_max_acc"] == pytest.approx(300.0)
def test_summarize_result_uses_report_metrics() -> None:
result = comparison.ReplayResult(
method="pinv",
times_s=np.asarray([0.0, 0.1]),
target_poses=np.zeros((2, 7)),
actual_poses=np.zeros((2, 7)),
joints=np.zeros((2, 7)),
velocities=np.asarray([[0.0] * 7, [math.pi] + [0.0] * 6]),
position_errors_m=np.asarray([0.003, 0.004]),
orientation_errors_rad=np.asarray([0.01, 0.02]),
joint_margins=np.asarray([0.2, 0.1]),
solve_durations_ms=np.asarray([1.0, 2.0]),
success=np.asarray([True, False]),
command_limited=np.asarray([False, True]),
)
actual = comparison.summarize_result(result)
assert actual.success_rate == pytest.approx(0.5)
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)