Add IK types, validation, and tests for RM75 kinematics

ik_qp/tests/test_kinematics.py

@@ -0,0 +1,64 @@
+import numpy as np
+import pinocchio as pin
+import pytest
+
+from rm75_ik import (
+    RM75Kinematics,
+    physical_joint_limits,
+    pose_errors,
+    teleop_joint_limits,
+)
+
+
+def test_limit_profiles_match_stage_one_contract():
+    physical = physical_joint_limits()
+    teleop = teleop_joint_limits()
+
+    np.testing.assert_allclose(
+        np.rad2deg(physical.upper), [178, 130, 178, 135, 178, 128, 360]
+    )
+    np.testing.assert_allclose(
+        np.rad2deg(teleop.lower), [-150, -30, -170, -130, -175, -125, -179]
+    )
+    np.testing.assert_allclose(
+        np.rad2deg(teleop.upper), [150, 110, 170, 130, 175, 125, 179]
+    )
+
+
+def test_zero_configuration_reaches_documented_flange_height():
+    kinematics = RM75Kinematics()
+    pose = kinematics.forward(np.zeros(7))
+
+    np.testing.assert_allclose(pose.translation, [0.0, 0.0, 0.8505], atol=3e-6)
+    np.testing.assert_allclose(pose.rotation, np.eye(3), atol=1e-7)
+
+
+def test_tool_pose_is_composed_after_flange():
+    kinematics = RM75Kinematics()
+    q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
+    tool = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.16]))
+
+    expected = kinematics.forward(q) * tool
+    actual = kinematics.forward(q, tool)
+
+    assert pose_errors(expected, actual) == pytest.approx((0.0, 0.0), abs=1e-12)
+
+
+@pytest.mark.parametrize(
+    "configuration",
+    [
+        np.zeros(6),
+        np.full(7, np.nan),
+        np.deg2rad([179, 0, 0, 0, 0, 0, 0]),
+    ],
+)
+def test_invalid_configuration_is_rejected(configuration):
+    with pytest.raises(ValueError):
+        RM75Kinematics().forward(configuration)
+
+
+def test_jacobian_has_expected_shape_and_is_finite():
+    jacobian = RM75Kinematics().jacobian(np.deg2rad([10, 20, -30, 40, 50, -60, 70]))
+    assert jacobian.shape == (6, 7)
+    assert np.all(np.isfinite(jacobian))
+