refactor(ik): joint limit helpers as module-level functions

src/roboticstoolbox/robot/IK.py

@@ -455,37 +455,13 @@ def _normalise_q(self, ets: "rtb.ETS", q: np.ndarray) -> None:
         :param q: Joint coordinates, indexed by joint jindex, modified in place
         :returns: None
 
-        Preserve coordinates already within their limits, including revolute joints
-        outside the principal interval. Otherwise prefer the principal angle, shifted
-        by whole turns towards the limits if necessary. If no equivalent angle lies
-        within the limits, the subsequent joint-limit check will reject the solution.
+        A thin wrapper which maps the jindex-indexed ``q`` onto the joints of the
+        ETS and applies :func:`normalise_q`.
         """
-        qlim = ets.qlim
-        for i, joint in enumerate(ets.joints()):
-            j = joint.jindex
-            lower, upper = qlim[:, i]
-            if not joint.isrotation or lower <= q[j] <= upper:
-                continue
-
-            # Avoid rounding a principal angle across a nearby joint limit.
-            angle = q[j]
-            if not -np.pi <= angle < np.pi:
-                angle = (angle + np.pi) % (2 * np.pi) - np.pi
-            if angle < lower:
-                angle += 2 * np.pi * np.ceil((lower - angle) / (2 * np.pi))
-            elif angle > upper:
-                angle -= 2 * np.pi * np.ceil((angle - upper) / (2 * np.pi))
-            if not lower <= angle <= upper:
-                # Argument reduction can round a limit plus whole turns just
-                # outside the closed interval. Accept that endpoint only when
-                # it reconstructs the original coordinate exactly, without an
-                # epsilon that could admit an unrelated out-of-limit angle.
-                for bound in (lower, upper):
-                    turns = np.rint((q[j] - bound) / (2 * np.pi))
-                    if turns != 0 and bound + turns * (2 * np.pi) == q[j]:
-                        angle = bound
-                        break
-            q[j] = angle
+        joints = ets.joints()
+        jindex = [joint.jindex for joint in joints]
+        revolute = np.array([joint.isrotation for joint in joints])
+        q[jindex] = normalise_q(q[jindex], ets.qlim, revolute)
 
     def _check_jl(self, ets: "rtb.ETS", q: np.ndarray) -> bool:
         """
@@ -496,22 +472,74 @@ def _check_jl(self, ets: "rtb.ETS", q: np.ndarray) -> bool:
         :returns: True if joints within feasible limits otherwise False
         :rtype: bool
 
+        A thin wrapper around :func:`within_limits`.
         """
+        return within_limits(q[: ets.n], ets.qlim)
 
-        # Loop through the joints in the ETS
-        for i in range(ets.n):
-            # Get the corresponding joint limits
-            ql0 = ets.qlim[0, i]
-            ql1 = ets.qlim[1, i]
 
-            # Check if q exceeds the limits
-            if q[i] < ql0 or q[i] > ql1:
-                return False
+def normalise_q(q: ArrayLike, qlim: ArrayLike, revolute: ArrayLike) -> NDArray:
+    """
+    Choose equivalent revolute joint coordinates, without changing prismatic joints
+
+    :param q: joint coordinates, shape (n,)
+    :param qlim: joint limits, shape (2, n), lower limits in the first row
+    :param revolute: True for each revolute joint, shape (n,)
+    :returns: the normalised joint coordinates, ``q`` is not modified
+    :rtype: ndarray(n)
+
+    Coordinates already within their limits are preserved, including revolute
+    joints outside the principal interval :math:`[-\\pi, \\pi)`. Otherwise the
+    principal angle is preferred, shifted by whole turns towards the limits if
+    necessary. If no equivalent angle lies within the limits the coordinate is
+    left as the nearest candidate, and a subsequent :func:`within_limits` check
+    will reject it.
+
+    This is the joint coordinate convention used by the numerical IK solvers.
+    """
+    q = np.array(q, dtype=float)
+    qlim = np.asarray(qlim, dtype=float)
+    revolute = np.asarray(revolute, dtype=bool)
+
+    for i in range(len(q)):
+        lower, upper = qlim[:, i]
+        if not revolute[i] or lower <= q[i] <= upper:
+            continue
+
+        # Avoid rounding a principal angle across a nearby joint limit.
+        angle = q[i]
+        if not -np.pi <= angle < np.pi:
+            angle = (angle + np.pi) % (2 * np.pi) - np.pi
+        if angle < lower:
+            angle += 2 * np.pi * np.ceil((lower - angle) / (2 * np.pi))
+        elif angle > upper:
+            angle -= 2 * np.pi * np.ceil((angle - upper) / (2 * np.pi))
+        if not lower <= angle <= upper:
+            # Argument reduction can round a limit plus whole turns just
+            # outside the closed interval. Accept that endpoint only when
+            # it reconstructs the original coordinate exactly, without an
+            # epsilon that could admit an unrelated out-of-limit angle.
+            for bound in (lower, upper):
+                turns = np.rint((q[i] - bound) / (2 * np.pi))
+                if turns != 0 and bound + turns * (2 * np.pi) == q[i]:
+                    angle = bound
+                    break
+        q[i] = angle
 
-        # If we make it here, all the joints are fine
-        return True
+    return q
 
 
+def within_limits(q: ArrayLike, qlim: ArrayLike) -> bool:
+    """
+    Test whether joint coordinates are within their limits
+
+    :param q: joint coordinates, shape (n,)
+    :param qlim: joint limits, shape (2, n), lower limits in the first row
+    :returns: True if every joint is within its limits (inclusive)
+    """
+    q = np.asarray(q, dtype=float)
+    qlim = np.asarray(qlim, dtype=float)
+    return not bool(np.any((q < qlim[0]) | (q > qlim[1])))
+
 def _null_Σ(ets: "rtb.ETS", q: NDArray, ps: float, pi: NDArray | float):
     """
     Formulates a relationship between joint limits and the joint velocity.

tests/test_ik_joint_limits.py

@@ -6,7 +6,7 @@
 
 import roboticstoolbox as rtb
 from roboticstoolbox.ets import fknm
-from roboticstoolbox.robot.IK import IKSolution
+from roboticstoolbox.robot.IK import IKSolution, normalise_q, within_limits
 from tests import skip_no_qp
 
 skip_no_c = pytest.mark.skipif(
@@ -220,3 +220,53 @@
     for q, target in zip(solution.q, targets):
         nt.assert_allclose(ets.fkine(q).A, target.A, atol=2e-6)
     nt.assert_allclose(solution.q[:, :3], qs[:, :3], atol=2e-6)
+
+
+# ---- the module-level helpers shared with other IK implementations ----------
+
+
+def test_normalise_q_preserves_coordinates_within_limits():
+    qlim = [[-10.0, -1.0], [10.0, 1.0]]
+    q = np.array([7.0, 0.5])  # revolute, outside the principal interval
+    nt.assert_array_equal(normalise_q(q, qlim, [True, True]), q)
+
+
+def test_normalise_q_shifts_into_limits_and_does_not_modify_input():
+    qlim = [[0.0, 0.0], [2 * np.pi, 2 * np.pi]]
+    q = np.array([-0.4, 1.0])
+    out = normalise_q(q, qlim, [True, True])
+    nt.assert_allclose(out, [2 * np.pi - 0.4, 1.0])
+    nt.assert_array_equal(q, [-0.4, 1.0])
+
+
+def test_normalise_q_prefers_principal_angle():
+    # 11 is outside the limits, the principal angle is 11 - 4 pi
+    out = normalise_q([11.0], [[-10.0], [10.0]], [True])
+    nt.assert_allclose(out, [11.0 - 4 * np.pi])
+
+
+def test_normalise_q_leaves_prismatic_alone():
+    out = normalise_q([50.0, 3.0], [[-1.0, -1.0], [1.0, 1.0]], [False, True])
+    assert out[0] == 50.0  # out of limits, but prismatic: never wrapped
+
+
+def test_normalise_q_without_equivalent_in_limits_is_rejected_by_check():
+    qlim = np.array([[0.2], [0.3]])
+    out = normalise_q([1.0], qlim, [True])
+    assert not within_limits(out, qlim)
+
+
+@pytest.mark.parametrize("turns", [-3, -1, 1, 3, 100])
+def test_normalise_q_keeps_limit_endpoint_after_whole_turns(turns):
+    qlim = np.array([[0.2], [0.3]])
+    out = normalise_q([0.2 + turns * 2 * np.pi], qlim, [True])
+    assert within_limits(out, qlim)
+    nt.assert_allclose(out, [0.2], atol=1e-12)
+
+
+def test_within_limits_is_inclusive():
+    qlim = [[-1.0, 0.0], [1.0, 2.0]]
+    assert within_limits([-1.0, 2.0], qlim)
+    assert within_limits([0.0, 1.0], qlim)
+    assert not within_limits([1.0 + 1e-12, 1.0], qlim)
+    assert not within_limits([0.0, -1e-12], qlim)