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))
+

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)
+

ik_qp/tests/test_solver.py

@@ -0,0 +1,53 @@
+import numpy as np
+import pinocchio as pin
+
+from rm75_ik import IkOptions, IkStatus, RM75IkSolver, RM75Kinematics, pose_errors
+
+
+def test_near_seed_round_trip_converges():
+    kinematics = RM75Kinematics()
+    solver = RM75IkSolver(kinematics)
+    target_q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
+    seed = target_q + np.deg2rad([2, -1, 2, -1, 1, -2, 3])
+
+    result = solver.solve(kinematics.forward(target_q), seed)
+
+    assert result.status is IkStatus.SUCCESS
+    assert result.q is not None
+    position_error, orientation_error = pose_errors(
+        kinematics.forward(result.q), kinematics.forward(target_q)
+    )
+    assert position_error <= 1e-3
+    assert orientation_error <= np.deg2rad(0.1)
+
+
+def test_invalid_seed_returns_no_solution():
+    kinematics = RM75Kinematics()
+    result = RM75IkSolver(kinematics).solve(
+        kinematics.forward(np.zeros(7)), np.full(7, np.nan)
+    )
+
+    assert result.status is IkStatus.INVALID_INPUT
+    assert result.q is None
+
+
+def test_invalid_rotation_returns_no_solution():
+    kinematics = RM75Kinematics()
+    invalid_target = pin.SE3(2.0 * np.eye(3), np.zeros(3))
+    result = RM75IkSolver(kinematics).solve(invalid_target, np.zeros(7))
+
+    assert result.status is IkStatus.INVALID_INPUT
+    assert result.q is None
+
+
+def test_expired_time_budget_returns_no_solution():
+    kinematics = RM75Kinematics()
+    result = RM75IkSolver(kinematics).solve(
+        kinematics.forward(np.deg2rad([30, 20, -40, 60, 50, -25, 90])),
+        np.zeros(7),
+        IkOptions(time_limit_sec=1e-12),
+    )
+
+    assert result.status is IkStatus.TIME_LIMIT
+    assert result.q is None
+

ik_qp/tests/test_validation_io.py

@@ -0,0 +1,35 @@
+import json
+from pathlib import Path
+
+from rm75_ik.validation import load_project_tools, write_validation_report
+
+
+def test_project_tool_config_and_report_output(tmp_path):
+    tools = load_project_tools(
+        Path(__file__).resolve().parents[2]
+        / "xr_rm_bringup"
+        / "config"
+        / "peripherals_rm75.yaml"
+    )
+    assert set(tools) == {"scissor", "omnipic", "minisci"}
+
+    summary = {
+        "passed": True,
+        "seed": 20260629,
+        "realman_api_version": "v1.1.5",
+        "failure_count": 0,
+        "checks": {
+            "example": {
+                "passed": True,
+                "required": True,
+                "samples": 1,
+            }
+        },
+    }
+    json_path, csv_path, markdown_path = write_validation_report(
+        tmp_path, summary, []
+    )
+
+    assert json.loads(json_path.read_text())["passed"] is True
+    assert csv_path.read_text().startswith("category,profile,sample")
+    assert "Overall: **PASS**" in markdown_path.read_text()