Enhance RM75 IK Solver with Joint Limit Regularization and Step Bounds

ik_qp/tests/test_solver.py

@@ -1,5 +1,6 @@
 import numpy as np
 import pinocchio as pin
+import pytest
 
 from rm75_ik import IkOptions, IkStatus, RM75IkSolver, RM75Kinematics, pose_errors
 
@@ -51,3 +52,65 @@ def test_expired_time_budget_returns_no_solution():
     assert result.status is IkStatus.TIME_LIMIT
     assert result.q is None
 
+
+def test_joint_limit_mid_regularization_points_toward_range_center():
+    kinematics = RM75Kinematics()
+    solver = RM75IkSolver(kinematics)
+    limits = kinematics.limits
+    midpoint = 0.5 * (limits.lower + limits.upper)
+    joint_range = limits.upper - limits.lower
+    options = IkOptions(
+        posture_weight=0.0,
+        joint_limit_mid_weight=2e-5,
+        joint_motion_weights=(1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2),
+    )
+
+    center_hessian, center_gradient = solver._regularization_terms(
+        midpoint, midpoint, options, damping=0.1
+    )
+    np.testing.assert_allclose(center_gradient, 0.0, atol=1e-15)
+
+    above = midpoint + 0.4 * joint_range
+    below = midpoint - 0.4 * joint_range
+    _, above_gradient = solver._regularization_terms(
+        above, above, options, damping=0.1
+    )
+    _, below_gradient = solver._regularization_terms(
+        below, below, options, damping=0.1
+    )
+    assert np.all(above_gradient > 0.0)
+    np.testing.assert_allclose(below_gradient, -above_gradient, rtol=1e-12)
+    assert center_hessian[6, 6] < center_hessian[0, 0]
+
+
+def test_per_joint_step_limits_and_hard_position_limits_are_combined():
+    kinematics = RM75Kinematics()
+    solver = RM75IkSolver(kinematics)
+    limits = kinematics.limits
+    midpoint = 0.5 * (limits.lower + limits.upper)
+    configured = np.array([0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10])
+    options = IkOptions(joint_step_limits_rad=tuple(configured))
+
+    lower, upper = solver._step_bounds(midpoint, options)
+    np.testing.assert_allclose(lower, -configured)
+    np.testing.assert_allclose(upper, configured)
+
+    near_upper = midpoint.copy()
+    near_upper[6] = limits.upper[6] - 0.01
+    lower, upper = solver._step_bounds(near_upper, options)
+    assert upper[6] == pytest.approx(0.01)
+    assert lower[6] == pytest.approx(-0.10)
+
+
+@pytest.mark.parametrize(
+    "kwargs",
+    [
+        {"joint_limit_mid_weight": -1.0},
+        {"joint_motion_weights": (1.0,) * 6 + (0.0,)},
+        {"joint_step_limits_rad": (0.05,) * 6},
+        {"joint_step_limits_rad": (0.05,) * 6 + (float("nan"),)},
+    ],
+)
+def test_invalid_joint_regularization_options_are_rejected(kwargs):
+    with pytest.raises(ValueError):
+        IkOptions(**kwargs)

ik_qp/tests/test_stage3_control.py

@@ -4,10 +4,9 @@ from pathlib import Path
 from types import SimpleNamespace
 
 import numpy as np
-import pinocchio as pin
 import pytest
 
-from rm75_ik import pose_errors
+from rm75_ik import IkResult, IkStatus, pose_errors
 from rm75_ik.mujoco_robot import MujocoRobot
 from rm75_ik.teleop_config import load_dual_arm_profiles
 from rm75_ik.teleop_control import (
@@ -50,6 +49,16 @@ def test_profiles_use_expected_tools_and_mapping(profiles):
         profiles["right"].xr_to_robot,
         [[0, 1, 0], [0, 0, 1], [1, 0, 0]],
     )
+    for profile in profiles.values():
+        assert profile.qp_w_limit_mid == pytest.approx(2e-5)
+        np.testing.assert_allclose(
+            profile.qp_joint_motion_weights,
+            [1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2],
+        )
+        np.testing.assert_allclose(
+            profile.qp_joint_step_limits_rad,
+            [0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10],
+        )
 
 
 @pytest.mark.parametrize(
@@ -139,6 +148,42 @@ def test_dual_controller_has_no_grip_jump_and_moves_both_arms(profiles):
         controller.close()
 
 
+def test_controller_passes_online_joint_regularization_options(
+    profiles, monkeypatch
+):
+    robot = MujocoRobot(profiles)
+    controller = DualArmQpTeleopController(robot, profiles)
+    captured = {}
+
+    def solve(target, seed, options):
+        del target
+        captured["options"] = options
+        return IkResult(
+            IkStatus.SUCCESS,
+            np.asarray(seed, dtype=float).copy(),
+            0.0,
+            0.0,
+            0,
+            0.0,
+        )
+
+    try:
+        monkeypatch.setattr(controller.solvers["left"], "solve", solve)
+        controller.update_sample(sample("left", True), 0.0)
+        assert controller.step(0.0).state is SafetyState.ACTIVE
+
+        options = captured["options"]
+        assert options.joint_limit_mid_weight == pytest.approx(2e-5)
+        assert options.joint_motion_weights == pytest.approx(
+            (1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2)
+        )
+        assert options.joint_step_limits_rad == pytest.approx(
+            (0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10)
+        )
+    finally:
+        controller.close()
+
+
 def test_active_arm_timeout_faults_both_and_requires_release(profiles):
     robot = MujocoRobot(profiles)
     controller = DualArmQpTeleopController(robot, profiles)