feat: 添加逆运动学对比指标

xr_rm_teleop/test/test_ik_method_comparison.py

@@ -102,3 +102,41 @@ def test_load_episode_rejects_wrong_arm(tmp_path: Path) -> None:
 
     with pytest.raises(ValueError, match="right_rm75"):
         comparison.load_episode(path)
+
+
+def test_orientation_error_and_joint_margin_match_definitions() -> None:
+    identity = np.eye(3)
+    quarter_turn = comparison._rotation_z(math.pi / 2.0)
+    joints = np.asarray([0.0, -0.5])
+    lower = np.asarray([-1.0, -1.0])
+    upper = np.asarray([1.0, 3.0])
+
+    assert comparison.orientation_error_rad(identity, quarter_turn) \
+        == pytest.approx(math.pi / 2.0)
+    assert comparison.normalized_joint_margin(joints, lower, upper) \
+        == pytest.approx(0.125)
+
+
+def test_choose_dls_damping_is_lexicographic() -> None:
+    candidates = [
+        comparison.MethodSummary("dls", 0.01, 0.90, 0.004, 0.01, 50.0),
+        comparison.MethodSummary("dls", 0.03, 0.95, 0.006, 0.02, 30.0),
+        comparison.MethodSummary("dls", 0.10, 0.95, 0.004, 0.01, 40.0),
+    ]
+
+    assert comparison.choose_dls_damping(candidates) == pytest.approx(0.10)
+
+
+def test_limit_joint_command_reuses_production_limiter() -> None:
+    target, velocity, limited = comparison.limit_joint_command(
+        target=np.full(7, 1.0),
+        previous_target=np.zeros(7),
+        previous_velocity=np.zeros(7),
+        max_speed=1.0,
+        max_acceleration=10.0,
+        dt=0.1,
+    )
+
+    assert target == pytest.approx([0.1] * 7)
+    assert velocity == pytest.approx([1.0] * 7)
+    assert limited