From e122ccb9a1ad003879a63ff0fa6d3accdcddde26 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E6=B0=B8=E7=A5=BA?= Date: Mon, 14 Sep 2026 10:28:28 +0800 Subject: [PATCH] fix: preserve joint types and limits in numerical IK --- docs/source/IK/ik.rst | 8 + src/roboticstoolbox/ets/cpp-extensions/ik.cpp | 50 +++- src/roboticstoolbox/robot/IK.py | 47 +++- tests/test_ik_joint_limits.py | 222 ++++++++++++++++++ 4 files changed, 319 insertions(+), 8 deletions(-) create mode 100644 tests/test_ik_joint_limits.py diff --git a/docs/source/IK/ik.rst b/docs/source/IK/ik.rst index 2e27013f3..8e934b8af 100644 --- a/docs/source/IK/ik.rst +++ b/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. diff --git a/src/roboticstoolbox/ets/cpp-extensions/ik.cpp b/src/roboticstoolbox/ets/cpp-extensions/ik.cpp index 8196ac455..46fb02a4e 100644 --- a/src/roboticstoolbox/ets/cpp-extensions/ik.cpp +++ b/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" */ \ No newline at end of file +} /* extern "C" */ diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index 48fcd87c6..232eb74cf 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/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 diff --git a/tests/test_ik_joint_limits.py b/tests/test_ik_joint_limits.py new file mode 100644 index 000000000..1811f88fb --- /dev/null +++ b/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)