Add IK types, validation, and tests for RM75 kinematics

ik_qp/tests/test_reference_and_dual.py

@@ -0,0 +1,67 @@
+import os
+from pathlib import Path
+
+import numpy as np
+import pinocchio as pin
+import pytest
+
+from rm75_ik import (
+    DualArmAssembly,
+    RM75Kinematics,
+    RealManFkReference,
+    pose_errors,
+)
+
+
+@pytest.fixture(scope="module")
+def reference():
+    sdk_root = os.environ.get("REALMAN_SDK_ROOT")
+    if not sdk_root:
+        pytest.skip("REALMAN_SDK_ROOT is not set")
+    return RealManFkReference(Path(sdk_root))
+
+
+@pytest.mark.parametrize(
+    "q_deg",
+    [
+        [0, 0, 0, 0, 0, 0, 0],
+        [30, -20, 40, 60, -50, 25, 90],
+        [-100, 80, -90, -70, 120, -60, -180],
+    ],
+)
+def test_pinocchio_fk_matches_realman_algo(reference, q_deg):
+    q = np.deg2rad(q_deg)
+    position_error, orientation_error = pose_errors(
+        RM75Kinematics().forward(q), reference.forward(q)
+    )
+
+    assert position_error < 1e-4
+    assert orientation_error < np.deg2rad(0.01)
+
+
+def test_tool_fk_matches_realman_algo(reference):
+    q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
+    tool = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.19]))
+    position_error, orientation_error = pose_errors(
+        RM75Kinematics().forward(q, tool), reference.forward(q, tool)
+    )
+
+    assert position_error < 1e-4
+    assert orientation_error < np.deg2rad(0.01)
+
+
+def test_dual_arm_mounts_reuse_single_arm_geometry(reference):
+    assembly = DualArmAssembly.from_source_urdf()
+    q = np.deg2rad([20, -10, 30, 40, -20, 15, 80])
+
+    assert assembly.dof == 14
+    assert assembly.mounts.right_visual_origin_delta_m == pytest.approx(0.001, abs=1e-8)
+    for arm, mount in (
+        ("left", assembly.mounts.left_base),
+        ("right", assembly.mounts.right_base),
+    ):
+        local = mount.actInv(assembly.forward(arm, q))
+        errors = pose_errors(local, reference.forward(q))
+        assert errors[0] < 1e-4
+        assert errors[1] < np.deg2rad(0.01)
+