fix: preserve joint types and limits in numerical IK

docs/source/IK/ik.rst

@@ -60,6 +60,14 @@ For example when using a 3 DOF manipulator tool orientation might be unimportant
 
 setting ``joint_limits = True`` will reject solutions with joint limit violations. Note that finding a solution with valid joint coordinates is likely to take longer than without.
 
+After convergence, prismatic joint coordinates are left unchanged. Revolute
+coordinates already within their limits are preserved, including limits outside
+the principal interval of :math:`[-\pi, \pi]`. Other revolute coordinates are
+reduced to the principal angle and, if necessary, shifted by whole turns towards
+their limits. If no equivalent angle is within the limits, the solution is
+rejected when ``joint_limits = True``. Setting ``joint_limits = False`` disables
+this rejection; it does not change a prismatic distance into an angle.
+
 .. rubric:: Others
 
 There are other arguments which may be unique to the solver, so check the documentation of the solver you wish to use for a complete list and explanation of arguments.

src/roboticstoolbox/ets/cpp-extensions/ik.cpp

@@ -51,8 +51,52 @@ static void _IK_loop(
 
             if (*E < tol)
             {
-                for (int i = 0; i < ets->n; i++)
-                    q(i) = std::fmod(q(i) + PI, PI_x2) - PI;
+                // Preserve translations and coordinates already within their limits.
+                // Other revolute coordinates may move only by whole turns, so the
+                // converged end-effector pose is unchanged (see IKSolver._normalise_q).
+                int j = 0;
+                for (int i = 0; i < ets->m; i++)
+                {
+                    ET *et = ets->ets[i];
+                    if (!et->isjoint)
+                        continue;
+
+                    double lower = ets->qlim_l[j];
+                    double upper = ets->qlim_h[j];
+                    if (et->axis < 3 && !(lower <= q(j) && q(j) <= upper))
+                    {
+                        // Avoid rounding a principal angle across a nearby limit.
+                        double angle = q(j);
+                        if (!(angle >= -PI && angle < PI))
+                        {
+                            // Unlike Python's %, fmod can return a negative remainder.
+                            angle = std::fmod(angle + PI, PI_x2);
+                            if (angle < 0)
+                                angle += PI_x2;
+                            angle -= PI;
+                        }
+                        if (angle < lower)
+                            angle += PI_x2 * std::ceil((lower - angle) / PI_x2);
+                        else if (angle > upper)
+                            angle -= PI_x2 * std::ceil((angle - upper) / PI_x2);
+                        if (!(lower <= angle && angle <= upper))
+                        {
+                            // Recover rounded endpoints only if whole turns
+                            // reconstruct the original coordinate exactly.
+                            for (double bound : {lower, upper})
+                            {
+                                double turns = std::round((q(j) - bound) / PI_x2);
+                                if (turns != 0 && bound + turns * PI_x2 == q(j))
+                                {
+                                    angle = bound;
+                                    break;
+                                }
+                            }
+                        }
+                        q(j) = angle;
+                    }
+                    j++;
+                }
                 *solution = reject_jl ? _check_lim(ets, q) : 1;
                 break;
             }
@@ -315,4 +359,4 @@ extern "C"
         // return q;
     }
 
-} /* extern "C" */
+} /* extern "C" */

src/roboticstoolbox/robot/IK.py

@@ -308,13 +308,10 @@ def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution:
             while True:
                 # Check convergence for the current q before another update.
                 if E < self.tol:
-                    # Wrap q to be within +- 180 deg
-                    # If your robot has larger than 180 deg range on a joint
-                    # this line should be modified in incorporate the extra range
-                    q = (q + np.pi) % (2 * np.pi) - np.pi
+                    self._normalise_q(ets, q)
 
                     # Check if we have violated joint limits
-                    jl_valid = self._check_jl(ets, q)
+                    jl_valid = self._check_jl(ets, q[ets.jindices])
 
                     if not jl_valid and self.joint_limits:
                         # Abandon search and try again
@@ -450,6 +447,46 @@ def _random_q(self, ets: "rtb.ETS", i: int = 1) -> np.ndarray:
 
         return q
 
+    def _normalise_q(self, ets: "rtb.ETS", q: np.ndarray) -> None:
+        """
+        Choose equivalent revolute coordinates without changing prismatic joints.
+
+        :param ets: The ETS defining the joint types and limits
+        :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.
+        """
+        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
+
     def _check_jl(self, ets: "rtb.ETS", q: np.ndarray) -> bool:
         """
         Checks if the joints are within their respective limits

tests/test_ik_joint_limits.py

@@ -0,0 +1,222 @@
+"""Joint-coordinate semantics shared by the numerical IK solvers."""
+
+import numpy as np
+import numpy.testing as nt
+import pytest
+
+import roboticstoolbox as rtb
+from roboticstoolbox.ets import fknm
+from roboticstoolbox.robot.IK import IKSolution
+from tests import skip_no_qp
+
+skip_no_c = pytest.mark.skipif(
+    not fknm._C_AVAILABLE, reason="compiled IK extension is not available"
+)
+PYTHON_SOLVERS = [
+    pytest.param("ikine_LM", {}, id="python-LM"),
+    pytest.param("ikine_GN", {}, id="python-GN"),
+    pytest.param("ikine_NR", {}, id="python-NR"),
+    pytest.param("ikine_QP", {}, marks=skip_no_qp, id="python-QP"),
+]
+C_SOLVERS = [
+    pytest.param("ik_LM", {"method": "chan"}, marks=skip_no_c, id="C-LM-chan"),
+    pytest.param("ik_LM", {"method": "wampler"}, marks=skip_no_c, id="C-LM-wampler"),
+    pytest.param("ik_LM", {"method": "sugihara"}, marks=skip_no_c, id="C-LM-sugihara"),
+    pytest.param("ik_GN", {}, marks=skip_no_c, id="C-GN"),
+    pytest.param("ik_NR", {}, marks=skip_no_c, id="C-NR"),
+]
+SOLVERS = PYTHON_SOLVERS + C_SOLVERS
+REPRESENTATIVE_SOLVERS = [PYTHON_SOLVERS[0], C_SOLVERS[0]]
+
+
+def _mixed_ets(
+    prismatic_limit: float = 10.0,
+    revolute_limits: tuple[float, float] = (-np.pi, np.pi),
+    jindices: list[int] | None = None,
+) -> rtb.ETS:
+    indices = [None] * 6 if jindices is None else jindices
+    return rtb.ETS(
+        [
+            rtb.ET.Rz(0.2),
+            rtb.ET.tx(
+                qlim=[-prismatic_limit, prismatic_limit],
+                flip=True,
+                jindex=indices[0],
+            ),
+            rtb.ET.ty(0.15),
+            rtb.ET.ty(qlim=[-10, 10], jindex=indices[1]),
+            rtb.ET.tz(qlim=[-10, 10], jindex=indices[2]),
+            rtb.ET.Rx(qlim=revolute_limits, jindex=indices[3]),
+            rtb.ET.tx(0.25),
+            rtb.ET.Ry(qlim=[-np.pi, np.pi], jindex=indices[4]),
+            rtb.ET.Rz(qlim=[-np.pi, np.pi], flip=True, jindex=indices[5]),
+        ]
+    )
+
+
+def _solve(
+    ets: rtb.ETS,
+    method: str,
+    options: dict[str, str],
+    q_target: np.ndarray,
+    q0: np.ndarray | None = None,
+    **kwargs: float,
+) -> IKSolution:
+    initial = q_target.copy() if q0 is None else q0.copy()
+    initial_before = initial.copy()
+    parameters: dict[str, object] = {
+        "q0": initial,
+        "ilimit": 1,
+        "slimit": 1,
+        "tol": 1e-12,
+    }
+    parameters.update(options)
+    parameters.update(kwargs)
+    solution = getattr(ets, method)(ets.fkine(q_target), **parameters)
+    nt.assert_array_equal(initial, initial_before)
+    return solution
+
+
+def _assert_valid_solution(
+    ets: rtb.ETS, solution: IKSolution, q_target: np.ndarray
+) -> None:
+    assert solution.success, solution.reason
+    nt.assert_allclose(ets.fkine(solution.q).A, ets.fkine(q_target).A, atol=2e-6)
+    assert np.all(solution.q >= ets.qlim[0])
+    assert np.all(solution.q <= ets.qlim[1])
+
+
+@pytest.mark.parametrize("method, options", SOLVERS)
+@pytest.mark.parametrize("displacement", [4.0, -4.0], ids=["positive", "negative"])
+def test_ik_preserves_prismatic_coordinates(
+    method: str, options: dict[str, str], displacement: float
+) -> None:
+    # Static ETs must not shift the joint numbering; flipped joints still store
+    # their own coordinate, rather than a signed or angularly wrapped surrogate.
+    ets = _mixed_ets()
+    q = np.array([displacement, 0.3, -0.2, 0.4, 0.5, 0.6])
+    solution = _solve(ets, method, options, q)
+    _assert_valid_solution(ets, solution, q)
+    nt.assert_array_equal(solution.q[:3], q[:3])
+
+
+@pytest.mark.parametrize("method, options", SOLVERS)
+def test_ik_preserves_valid_multi_turn_revolute_coordinates(
+    method: str, options: dict[str, str]
+) -> None:
+    ets = _mixed_ets(revolute_limits=(-4 * np.pi, 4 * np.pi))
+    q = np.array([0.4, 0.3, -0.2, 2 * np.pi + 0.4, 0.5, 0.6])
+    solution = _solve(ets, method, options, q)
+    _assert_valid_solution(ets, solution, q)
+    nt.assert_allclose(solution.q, q, atol=1e-12)
+
+
+@pytest.mark.parametrize("method, options", REPRESENTATIVE_SOLVERS)
+@pytest.mark.parametrize(
+    "limits, angle, expected",
+    [
+        pytest.param((3.5, 5.5), 4.0, 4.0, id="valid-offset-range"),
+        pytest.param((3.5, 5.5), 4.0 + 2 * np.pi, 4.0, id="equivalent-offset-range"),
+        pytest.param((-5.5, -3.5), -4.0 - 2 * np.pi, -4.0, id="negative-offset-range"),
+        pytest.param(
+            (-np.pi, np.pi), -4 * np.pi + 0.4, 0.4, id="negative-multiple-turns"
+        ),
+        pytest.param((-np.pi, np.pi), np.pi, np.pi, id="positive-boundary"),
+        pytest.param((-np.pi, np.pi), -np.pi, -np.pi, id="negative-boundary"),
+        pytest.param((0.1, 1.0), 0.1 + 2 * np.pi, 0.1, id="lower-boundary-turn"),
+        pytest.param((0.2, 0.3), 0.3 + 2 * np.pi, 0.3, id="upper-boundary-turn"),
+        pytest.param((-1.0, -0.1), -0.1 - 2 * np.pi, -0.1, id="negative-boundary-turn"),
+    ],
+)
+def test_ik_selects_equivalent_revolute_coordinate_in_limits(
+    method: str,
+    options: dict[str, str],
+    limits: tuple[float, float],
+    angle: float,
+    expected: float,
+) -> None:
+    ets = _mixed_ets(revolute_limits=limits)
+    q = np.array([0.4, 0.3, -0.2, angle, 0.5, 0.6])
+    solution = _solve(ets, method, options, q)
+    _assert_valid_solution(ets, solution, q)
+    nt.assert_allclose(solution.q[3], expected, atol=1e-12)
+
+
+@pytest.mark.parametrize("method, options", SOLVERS)
+def test_ik_does_not_wrap_invalid_prismatic_coordinate_into_limits(
+    method: str, options: dict[str, str]
+) -> None:
+    ets = _mixed_ets(prismatic_limit=1.0)
+    q = np.array([2 * np.pi + 0.4, 0.3, -0.2, 0.4, 0.5, 0.6])
+    rejected = _solve(ets, method, options, q, joint_limits=True)
+    assert not rejected.success
+
+    # Disabling limit rejection allows this same pose, but must not change the
+    # achieved translation by treating the coordinate as a periodic angle.
+    accepted = _solve(ets, method, options, q, joint_limits=False)
+    assert accepted.success
+    nt.assert_allclose(ets.fkine(accepted.q).A, ets.fkine(q).A, atol=1e-12)
+    nt.assert_array_equal(accepted.q[:3], q[:3])
+
+
+@pytest.mark.parametrize("method, options", REPRESENTATIVE_SOLVERS)
+@pytest.mark.parametrize(
+    "angle", [1.0, np.nextafter(0.2, -np.inf), np.nextafter(0.3, np.inf)]
+)
+def test_ik_rejects_revolute_pose_without_equivalent_coordinate_in_limits(
+    method: str, options: dict[str, str], angle: float
+) -> None:
+    ets = _mixed_ets(revolute_limits=(0.2, 0.3))
+    q = np.array([0.4, 0.3, -0.2, angle, 0.5, 0.6])
+    solution = _solve(ets, method, options, q)
+    assert not solution.success
+
+
+@pytest.mark.parametrize("method, options", SOLVERS)
+def test_ik_preserves_translation_after_solver_iterations(
+    method: str, options: dict[str, str]
+) -> None:
+    ets = _mixed_ets()
+    q = np.array([4.0, 0.3, -0.2, 0.4, 0.5, 0.6])
+    q0 = q + np.array([0.2, -0.1, 0.1, 0.02, -0.02, 0.01])
+    solution = _solve(ets, method, options, q, q0=q0, ilimit=100)
+    _assert_valid_solution(ets, solution, q)
+    assert solution.iterations > 0
+    nt.assert_allclose(solution.q[:3], q[:3], atol=2e-6)
+
+
+@pytest.mark.parametrize("method, options", PYTHON_SOLVERS)
+def test_ik_joint_types_follow_sparse_joint_indices(
+    method: str, options: dict[str, str]
+) -> None:
+    # The solver works internally with a padded global coordinate vector, while
+    # public q0 and IKSolution.q contain only this branch's active joints.
+    ets = _mixed_ets(jindices=[1, 3, 5, 7, 9, 11])
+    q = np.array([4.0, 0.3, -0.2, 0.4, 0.5, 0.6])
+    solution = _solve(ets, method, options, q)
+    _assert_valid_solution(ets, solution, q)
+    assert solution.q.shape == (6,)
+    nt.assert_allclose(solution.q, q, atol=1e-12)
+
+    limited = _mixed_ets(prismatic_limit=1.0, jindices=[1, 3, 5, 7, 9, 11])
+    invalid_q = q.copy()
+    invalid_q[0] = 2 * np.pi + 0.4
+    rejected = _solve(limited, method, options, invalid_q)
+    assert not rejected.success
+
+
+@pytest.mark.parametrize("method, options", PYTHON_SOLVERS)
+def test_ik_trajectory_retains_prismatic_coordinates(
+    method: str, options: dict[str, str]
+) -> None:
+    ets = _mixed_ets()
+    qs = np.array([[4.0, 0.3, -0.2, 0.4, 0.5, 0.6], [4.1, 0.2, -0.1, 0.4, 0.5, 0.6]])
+    targets = ets.fkine(qs)
+    solution = getattr(ets, method)(
+        targets, q0=qs[0], ilimit=100, slimit=1, tol=1e-12, **options
+    )
+    assert solution.success, solution.reason
+    assert solution.q.shape == (2, 6)
+    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)