From 5eabf271ca692532ff55353fa85f133e17eb8ec6 Mon Sep 17 00:00:00 2001 From: qwist1233-cpu Date: Sat, 17 Oct 2026 11:28:01 +0300 Subject: [PATCH] feat(ets): add numerical inverse kinematics for planar ETS2 ETS2 had fkine, jacob0 and jacobe but no inverse kinematics, so planar robots could only be solved by hand. Add ETS2.ikine_LM, a Levenberg- Marquardt solver with Chan's damping (the planar counterpart of ETS.ikine_LM): 3-vector (x, y, theta) pose error, optional mask, random restarts within the joint limits, joint-limit rejection and trajectory support, returning the existing IKSolution dataclass. Closes #824 Co-Authored-By: Claude Fable 5.1 --- docs/source/ets_2d.rst | 8 +- src/roboticstoolbox/ets/ETS2.py | 250 ++++++++++++++++++++++++++++++++ tests/test_ets2_ikine_LM.py | 132 +++++++++++++++++ 3 files changed, 387 insertions(+), 3 deletions(-) create mode 100644 tests/test_ets2_ikine_LM.py diff --git a/docs/source/ets_2d.rst b/docs/source/ets_2d.rst index 88eecaf1d..69df3f94a 100644 --- a/docs/source/ets_2d.rst +++ b/docs/source/ets_2d.rst @@ -16,8 +16,9 @@ We can use :class:`~roboticstoolbox.ETS` to describes a planar arm that moves in >>> print(e) >>> e.fkine([0.3, 0.4]) -.. note:: ``ETS2`` has no inverse kinematics, Hessian or manipulability - methods yet, see `Issue 824 `_. +.. note:: ``ETS2`` has numerical inverse kinematics, + :meth:`~roboticstoolbox.ETS2.ikine_LM`, but no Hessian or manipulability + methods yet. ET2: Elementary Transform ------------------------- @@ -158,6 +159,7 @@ Kinematics ~roboticstoolbox.ETS2.eval ~roboticstoolbox.ETS2.jacob0 ~roboticstoolbox.ETS2.jacobe + ~roboticstoolbox.ETS2.ikine_LM Other ^^^^^ @@ -172,5 +174,5 @@ Details ^^^^^^^ .. autoclass:: roboticstoolbox.ETS2 - :members: n, m, structure, joints, joint_idx, jindices, jindex_set, qlim, random_q, insert, append, extend, remove, reverse, inv, fixjoint, split, swap, merge, compile, fkine, eval, jacob0, jacobe, plot, teach + :members: n, m, structure, joints, joint_idx, jindices, jindex_set, qlim, random_q, insert, append, extend, remove, reverse, inv, fixjoint, split, swap, merge, compile, fkine, eval, jacob0, jacobe, ikine_LM, plot, teach :member-order: bysource diff --git a/src/roboticstoolbox/ets/ETS2.py b/src/roboticstoolbox/ets/ETS2.py index 781fda0d1..06ff7b63e 100644 --- a/src/roboticstoolbox/ets/ETS2.py +++ b/src/roboticstoolbox/ets/ETS2.py @@ -19,6 +19,7 @@ from roboticstoolbox.ets._ETS import BaseETS from roboticstoolbox.ets.ET2 import ET2 +from roboticstoolbox.robot.IK import IKSolution, normalise_q, within_limits from roboticstoolbox.tools.types import ArrayLike, NDArray @@ -480,3 +481,252 @@ def jacobe(self, q: ArrayLike) -> Any: T = self.fkine(q, include_base=False).A return tr2jac2(T.T) @ self.jacob0(q) + + def ikine_LM( + self, + Tep: NDArray | SE2, + q0: ArrayLike | None = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: ArrayLike | None = None, + joint_limits: bool = True, + seed: int | None = None, + k: float = 1.0, + ) -> IKSolution: + r""" + Levenberg-Marquardt numerical inverse kinematics solver + + :param Tep: the desired end-effector pose, an ``SE2`` or a 3x3 array, or a + pose trajectory, an ``SE2`` with several values or an (N, 3, 3) array + :param q0: the initial joint coordinates, a vector (n,), or a matrix (m, n) + whose rows are the starting points of the first m searches (any + further searches start from random coordinates within the joint + limits) + :param ilimit: maximum iterations allowed per search + :param slimit: maximum search attempts before failure + :param tol: maximum allowed residual error E + :param mask: a 3-vector weighting the (x, y, θ) error priority, for + example ``[1, 1, 0]`` to solve for position only + :param joint_limits: reject solutions with joint limit violations + :param seed: seed for the RNG used to generate random joint configurations + :param k: gain λ of Chan's damping matrix, see below + :returns: the solution, see :class:`~roboticstoolbox.robot.IK.IKSolution` + :raises ValueError: ``Tep`` is not a 3x3 pose or an (N, 3, 3) trajectory, + ``mask`` is not a 3-vector, ``q0`` has the wrong number of columns, or + a joint has an infinite limit so no random configuration can be drawn + + ``sol = ets.ikine_LM(Tep)`` are the joint coordinates (n) corresponding to + the planar end-effector pose ``Tep``. The solver is the planar counterpart + of :meth:`ETS.ikine_LM`: the pose error is the 3-vector + :math:`\vec{e} = (\Delta x, \Delta y, \Delta\theta)` in the world frame, + the residual is :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` + with :math:`\mat{W}_e = \text{diag}(\text{mask})`, and each step is + + .. math:: + + \vec{q}_{k+1} = \vec{q}_k + + \left( \mat{J}^\top \mat{W}_e \mat{J} + \lambda E_k \mat{1}_n \right)^{-1} + \mat{J}^\top \mat{W}_e \vec{e} + + where :math:`\mat{J}` is :meth:`jacob0`. Because `E` is quadratic, `tol` + bounds the pose error only to roughly :math:`\sqrt{2 \cdot \text{tol}}`; + tighten it if a specific linear-scale accuracy is needed. + + Each search starts from a row of ``q0``, or a random configuration drawn + within the joint limits, and runs for at most ``ilimit`` iterations; up + to ``slimit`` searches are made before the solver gives up. A solution + that lies outside the joint limits is rejected when ``joint_limits`` is + True, and the search continues. + + **Trajectory operation**: for a trajectory of N poses the solver is run + once per pose, each from the same starting points, and the returned + ``q`` has shape (N, n). ``success`` is True only if every pose was + solved, ``iterations`` and ``searches`` are summed, ``residual`` is the + largest over the poses and ``reason`` is that of the last failed pose. + + Example: + + .. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> from roboticstoolbox.ets.ET2 import R, tx + >>> ets = R() * tx(1) * R() * tx(1) + >>> Tep = ets.fkine([0.3, 0.7]) + >>> sol = ets.ikine_LM(Tep, seed=0) + >>> sol.success + >>> ets.fkine(sol.q) + + .. rubric:: References + + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + + .. seealso:: :meth:`fkine` :meth:`jacob0` :meth:`ETS.ikine_LM` + + .. versionadded:: 1.4.5 + """ + if mask is None: + We = np.eye(3) + else: + m = np.asarray(mask, dtype=float).flatten() + if m.shape != (3,): + raise ValueError("mask must be a 3-vector (x, y, θ)") + We = np.diag(m) + + traj = False + targets: NDArray + + if isinstance(Tep, SE2): + if len(Tep) > 1: + traj = True + targets = np.array([T.A for T in Tep], dtype=float) + else: + targets = np.asarray(Tep.A, dtype=float)[np.newaxis] + else: + Tep = np.asarray(Tep, dtype=float) + if Tep.ndim == 3 and Tep.shape[1:] == (3, 3): + traj = True + targets = Tep + elif Tep.shape == (3, 3): + targets = Tep[np.newaxis] + else: + raise ValueError( + "Tep must be an SE2, a 3x3 array or an (N, 3, 3) array" + ) + + n = self.n + qlim = self.qlim + + if not np.all(np.isfinite(qlim)): + bad = np.flatnonzero(~np.all(np.isfinite(qlim), axis=0)) + raise ValueError( + f"Joint limit(s) for joint index(es) {bad.tolist()} are not " + f"finite (qlim={qlim[:, bad].tolist()}) -- can't generate a " + "random configuration within an infinite/undefined range." + ) + + rng = np.random.default_rng(seed) + starts = rng.uniform(qlim[0], qlim[1], size=(slimit, n)) + + if q0 is not None: + q0a = np.asarray(q0, dtype=float) + if q0a.ndim == 1: + q0a = q0a[np.newaxis] + if q0a.ndim != 2 or q0a.shape[1] != n: + raise ValueError(f"q0 must have {n} columns, one per joint") + rows = min(q0a.shape[0], slimit) + starts[:rows] = q0a[:rows] + + revolute = np.array([joint.isrotation for joint in self.joints()]) + + def solve_one(T: NDArray) -> IKSolution: + theta_d = np.arctan2(T[1, 0], T[0, 0]) + + def error(q: NDArray) -> tuple[NDArray, float]: + Te = self.eval(q) + dtheta = theta_d - np.arctan2(Te[1, 0], Te[0, 0]) + e = np.array( + [ + T[0, 2] - Te[0, 2], + T[1, 2] - Te[1, 2], + np.arctan2(np.sin(dtheta), np.cos(dtheta)), + ] + ) + return e, float(0.5 * e @ We @ e) + + total_i = 0 + found_with_limits = False + linalg_error = 0 + E = np.inf + q = starts[0] + + for search in range(slimit): + q = starts[search].copy() + i = 0 + + # Check the starting point before taking a step, which can be + # singular even though the requested pose is already reached. + e, E = error(q) + + while True: + if E < tol: + q = normalise_q(q, qlim, revolute) + + if joint_limits and not within_limits(q, qlim): + found_with_limits = True + break + + return IKSolution( + q=q, + success=True, + iterations=total_i + i, + searches=search + 1, + residual=E, + reason="Success", + ) + + if i >= ilimit: + break + + i += 1 + + J = self.jacob0(q) + A = J.T @ We @ J + k * E * np.eye(n) + g = J.T @ We @ e + + try: + q = q + np.linalg.solve(A, g) + except np.linalg.LinAlgError: + linalg_error += 1 + break + + e, E = error(q) + + total_i += i + + reason = "iteration and search limit reached" + + if linalg_error: + reason += f", {linalg_error} numpy.LinAlgError encountered" + + if found_with_limits: + reason += ", solution found but violates joint limits" + + return IKSolution( + q=q, + success=False, + iterations=total_i, + searches=slimit, + residual=E, + reason=reason, + ) + + if not traj: + return solve_one(targets[0]) + + q = np.empty((targets.shape[0], n)) + success = True + iterations = 0 + searches = 0 + residual = 0.0 + reason = "" + + for i, T in enumerate(targets): + sol = solve_one(T) + q[i] = sol.q + if not sol.success: + success = False + reason = sol.reason + iterations += sol.iterations + searches += sol.searches + residual = max(residual, sol.residual) + + return IKSolution( + q=q, + success=success, + iterations=iterations, + searches=searches, + residual=residual, + reason=reason, + ) diff --git a/tests/test_ets2_ikine_LM.py b/tests/test_ets2_ikine_LM.py new file mode 100644 index 000000000..74667634e --- /dev/null +++ b/tests/test_ets2_ikine_LM.py @@ -0,0 +1,132 @@ +""" +Tests for ETS2.ikine_LM, the planar numerical inverse kinematics solver. +""" + +import numpy as np +import numpy.testing as nt +import pytest +from spatialmath import SE2 + +from roboticstoolbox.ets.ET2 import R, tx +from roboticstoolbox.robot.IK import IKSolution + + +def two_link(): + return R() * tx(1) * R() * tx(1) + + +def three_link(): + return R() * tx(1) * R() * tx(1) * R() * tx(0.5) + + +def pose_error(ets, q, Tep): + Te = np.asarray(ets.fkine(q).A, dtype=float) + Tep = np.asarray(Tep, dtype=float) + d = np.arctan2(Tep[1, 0], Tep[0, 0]) - np.arctan2(Te[1, 0], Te[0, 0]) + return np.array( + [Tep[0, 2] - Te[0, 2], Tep[1, 2] - Te[1, 2], np.arctan2(np.sin(d), np.cos(d))] + ) + + +def test_two_link_reaches_pose(): + ets = two_link() + Tep = ets.fkine([0.3, 0.7]) + sol = ets.ikine_LM(Tep, seed=0, tol=1e-12) + assert isinstance(sol, IKSolution) + assert sol.success + assert sol.reason == "Success" + assert sol.q.shape == (2,) + nt.assert_allclose(pose_error(ets, sol.q, Tep.A), 0, atol=1e-5) + + +def test_accepts_ndarray_target(): + ets = three_link() + Tep = ets.fkine([0.2, -0.4, 0.9]).A + sol = ets.ikine_LM(Tep, seed=1, tol=1e-12) + assert sol.success + nt.assert_allclose(pose_error(ets, sol.q, Tep), 0, atol=1e-5) + + +def test_initial_guess_converges_in_one_search(): + ets = three_link() + q_true = np.array([0.2, -0.4, 0.9]) + Tep = ets.fkine(q_true) + sol = ets.ikine_LM(Tep, q0=q_true + 0.05, seed=3, tol=1e-12) + assert sol.success + assert sol.searches == 1 + nt.assert_allclose(sol.q, q_true, atol=1e-4) + + +def test_initial_guess_matrix(): + ets = two_link() + Tep = ets.fkine([0.3, 0.7]) + sol = ets.ikine_LM( + Tep, q0=np.array([[2.0, 2.0], [0.3, 0.7]]), ilimit=1, slimit=2, tol=1e-12 + ) + assert sol.success + assert sol.searches == 2 + + +def test_mask_position_only(): + # a 2-link arm can't reach an arbitrary (x, y, θ), so only position + ets = two_link() + Tep = SE2(1.2, 0.8, 2.5) + assert not ets.ikine_LM(Tep, seed=0, slimit=10).success + sol = ets.ikine_LM(Tep, mask=[1, 1, 0], seed=0, tol=1e-12) + assert sol.success + nt.assert_allclose(ets.fkine(sol.q).t, [1.2, 0.8], atol=1e-5) + + +def test_joint_limits(): + ets = R() * tx(1) * R(qlim=[-1, 1]) * tx(1) + Tep = ets.fkine([0.2, 2.0]) # q2 = 2.0 is outside its limit + sol = ets.ikine_LM(Tep, seed=0, slimit=20) + assert not sol.success + assert "violates joint limits" in sol.reason + sol = ets.ikine_LM(Tep, seed=0, joint_limits=False, tol=1e-12) + assert sol.success + nt.assert_allclose(pose_error(ets, sol.q, Tep.A), 0, atol=1e-5) + + +def test_unreachable_target(): + ets = two_link() + sol = ets.ikine_LM(SE2(5, 0, 0), mask=[1, 1, 0], seed=0, ilimit=10, slimit=5) + assert not sol.success + assert sol.searches == 5 + assert sol.reason.startswith("iteration and search limit reached") + + +def test_seed_is_reproducible(): + ets = three_link() + Tep = ets.fkine([1.0, 1.0, -0.5]) + a = ets.ikine_LM(Tep, seed=7) + b = ets.ikine_LM(Tep, seed=7) + nt.assert_array_equal(a.q, b.q) + assert a.iterations == b.iterations + + +def test_trajectory(): + ets = three_link() + qs = np.array([[0.1, 0.2, 0.3], [0.4, -0.2, 0.1], [-0.3, 0.5, 0.2]]) + Tep = ets.fkine(qs) + assert len(Tep) == 3 + sol = ets.ikine_LM(Tep, seed=0, tol=1e-12) + assert sol.success + assert sol.q.shape == (3, 3) + for q, T in zip(sol.q, Tep): + nt.assert_allclose(pose_error(ets, q, T.A), 0, atol=1e-5) + + sol2 = ets.ikine_LM(np.array([T.A for T in Tep]), seed=0, tol=1e-12) + assert sol2.success + assert sol2.q.shape == (3, 3) + + +def test_bad_arguments(): + ets = two_link() + Tep = ets.fkine([0.3, 0.7]) + with pytest.raises(ValueError): + ets.ikine_LM(Tep, mask=[1, 1]) + with pytest.raises(ValueError): + ets.ikine_LM(np.eye(4)) + with pytest.raises(ValueError): + ets.ikine_LM(Tep, q0=[0.1, 0.2, 0.3])