From 718efffef734078d2bed836ac3cbbdf903b8230e Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 4 Oct 2026 16:52:55 -0400 Subject: [PATCH] fix(puma): self-contained ikine_a, fix its trajectory and failure handling; deprecate ikine_6s Puma560.ikine_a relied on DHRobot.ikine_6s, which came from the MATLAB Toolbox and is not general: it only handled the wrist, while the Paul and Zhang first-three-joints solution (ik3) is specific to the Puma geometry (zero first link length, lateral shoulder offset d3). Move the wrist, base/tool and trajectory handling into ikine_a so it is self-contained, and deprecate ikine_6s (frozen, it still runs but warns). Fix bugs in the old path: - a pose closer to the waist axis than the shoulder offset returned success=True with q all NaN (the arcsin(d3/r) NaN was not caught) - a trajectory returned a garbled IKSolution: the failure reasons were passed as the iterations argument, success was an array, and reason was empty - a trajectory containing an unreachable pose raised ValueError - an ndarray pose raised AttributeError - the docstring said it returned a joint vector, and had typos ikine_a now returns an IKSolution like the numerical solvers: q (n,) or (N, n), one bool success, the maximum residual, and the last failure reason. Joints of a pose that cannot be solved are NaN. Success is decided by checking the solution by forward kinematics, not by the absence of an exception. Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/models/DH/Puma560.py | 140 ++++++++++++++++++++--- src/roboticstoolbox/robot/DHRobot.py | 24 ++++ tests/test_DHRobot.py | 105 +++++++++++++++++ 3 files changed, 255 insertions(+), 14 deletions(-) diff --git a/src/roboticstoolbox/models/DH/Puma560.py b/src/roboticstoolbox/models/DH/Puma560.py index 33b7313ad..d5ec4e4ce 100755 --- a/src/roboticstoolbox/models/DH/Puma560.py +++ b/src/roboticstoolbox/models/DH/Puma560.py @@ -17,7 +17,8 @@ # from math import pi import numpy as np -from roboticstoolbox import DHRobot, RevoluteDH +from roboticstoolbox import DHRobot, RevoluteDH, IKSolution, angle_axis +from roboticstoolbox.tools.types import NDArray from spatialmath import SE3 from spatialmath import base @@ -200,20 +201,22 @@ def __init__(self, symbolic=False): # straight and horizontal self.addconfiguration_attr("qs", np.array([0, 0, -pi / 2, 0, 0, 0])) - def ikine_a(self, T, config="lun"): - """ + def ikine_a( + self, T: SE3 | NDArray, config: str = "lun", tol: float = 1e-6 + ) -> IKSolution: + r""" Analytic inverse kinematic solution - :param T: end-effector pose - :type T: SE3 + :param T: end-effector pose, or a trajectory of poses :param config: arm configuration, defaults to "lun" - :type config: str, optional - :return: joint angle vector in radians - :rtype: ndarray(6) + :param tol: maximum allowed residual error E, as for the numerical IK + solvers, used to decide whether a solution reaches the pose + :returns: an IKSolution containing joint coordinates ``q``, ``success`` + flag, ``residual`` error value, and ``reason`` string if applicable - ``robot.ikine_a(T, config)`` is the joint angle vector which achieves the - end-effector pose ``T```. The configuration string selects the specific - solution and is a sting comprising the following letters: + ``robot.ikine_a(T, config)`` is the joint coordinates which achieve the + end-effector pose ``T``. The configuration string selects the specific + solution and is a string comprising the following letters: ====== ============================================== Letter Meaning @@ -226,6 +229,34 @@ def ikine_a(self, T, config="lun"): f Choose the wrist flipped configuration ====== ============================================== + ``T`` is an :class:`SE3`, or a 4x4 array, for a single pose. If it is an + :class:`SE3` holding N poses, or an array with shape (N, 4, 4), it is a + trajectory and each pose is solved independently with the same ``config``. + The result is the same as for the numerical IK solvers: + + - ``q`` has shape (n,) for a single pose, or (N, n) for a trajectory + - ``success`` is True only if every pose was solved + - ``residual`` is the maximum residual over the poses, and is infinite if + any pose is out of reach + - ``reason`` is the reason given by the last pose that failed, if any + - ``iterations`` and ``searches`` are zero, this is not an iterative method + + The joints of a pose that cannot be solved are NaN. Every solution is + checked by forward kinematics, so ``success`` is True only if the + returned ``q`` reaches the pose to within ``tol``. + + This is a hand-written solution for the Puma 560 geometry, in particular + a zero first link length and a lateral shoulder offset :math:`d_3`. It + does not apply to other robots. Joint limits are not considered. + + .. versionchanged:: 1.5.0 + Accepts an array as well as an :class:`SE3`, returns a single + :class:`IKSolution` for a trajectory like the numerical solvers (it + was garbled, and failed for an unreachable pose), reports a pose + closer to the waist axis than the shoulder offset as out of reach + (it returned ``success=True`` with NaN joint coordinates), and checks + the solution by forward kinematics. It no longer uses + :meth:`DHRobot.ikine_6s`, which is deprecated. :reference: - Inverse kinematics for a PUMA 560, @@ -238,9 +269,9 @@ def ikine_a(self, T, config="lun"): """ - def ik3(robot, T, config="lun"): + config = self.config_validate(config, ("lr", "ud", "nf")) - config = self.config_validate(config, ("lr", "ud", "nf")) + def ik3(robot, T, config="lun"): # solve for the first three joints @@ -263,6 +294,10 @@ def ik3(robot, T, config="lun"): # based on the configuration parameter n1 r = np.sqrt(Px**2 + Py**2) + if r < abs(d3) or r == 0: + # the wrist centre is closer to the waist axis than the + # shoulder offset, so arcsin(d3 / r) below has no solution + return "Out of reach" if "r" in config: theta[0] = np.arctan2(Py, Px) + np.arcsin(d3 / r) elif "l" in config: @@ -310,7 +345,84 @@ def ik3(robot, T, config="lun"): return theta - return self.ikine_6s(T, config, ik3) + # spatialmath cannot build an SE3 from an (N, 4, 4) array, see + # https://github.com/rai-opensource/spatialmath-python/issues/236 + if isinstance(T, np.ndarray): + if T.shape != (4, 4) and not (T.ndim == 3 and T.shape[1:] == (4, 4)): + raise ValueError("T must be an SE3, a 4x4 array or an (N, 4, 4) array") + T = SE3(list(T) if T.ndim == 3 else T, check=False) + + # undo the base and tool transformations, skipped if they are not set + # (careful: ``base`` is the spatialmath module in ik3 above) + T_arm = T + if not np.array_equal(self.base.A, np.eye(4)): + T_arm = self.base.inv() * T_arm + if not np.array_equal(self.tool.A, np.eye(4)): + T_arm = T_arm * self.tool.inv() + + q = np.full((len(T), self.n), np.nan) + residual = 0.0 + reason = "" + success = True + + for k, (Tk, Tk_arm) in enumerate(zip(T, T_arm)): + # solve for the first three joints + theta = ik3(self, Tk_arm, config) + + if isinstance(theta, str): + qk = None + why = theta + else: + # Solve for the wrist rotation. We need to account for some + # translations between the first and last 3 joints (d4) and + # also d6, a6, alpha6 in the final frame. + + # transform of the first 3 joints + T13 = self.A([0, 2], theta) + + # T = T13 * Tz(d4) * R * Tz(d6) Tx(a5) + Td4 = SE3(0, 0, self.links[3].d) # Tz(d4) + + # Tz(d6) Tx(a5) Rx(alpha6) + Tt = SE3(self.links[5].a, 0, self.links[5].d) * SE3.Rx( + self.links[5].alpha + ) + + R = Td4.inv() * T13.inv() * Tk_arm * Tt.inv() + + # the spherical wrist implements Euler angles + eul = R.eul(flip=True) if "f" in config else R.eul() + qk = np.r_[theta, eul] + if self.links[3].alpha > 0: + qk[4] = -qk[4] + + # remove the link offset angles + qk = qk - self.offset + why = "" + + if qk is None or not np.all(np.isfinite(qk)): + # out of reach, there is no solution at all + E = np.inf + why = why or "Out of reach" + else: + # check the solution, success is not the absence of an exception + e = angle_axis(self.fkine(qk).A, Tk.A) + E = 0.5 * float(e @ e) + q[k] = qk + if E > tol: + why = f"Solution does not reach the pose, residual {E:.3g}" + + if E > tol or why: + success = False + reason = why + residual = max(residual, E) + + return IKSolution( + q=q[0] if len(T) == 1 else q, + success=success, + residual=residual, + reason=reason, + ) if __name__ == "__main__": # pragma nocover diff --git a/src/roboticstoolbox/robot/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index 5e2d5f431..dbb4129a0 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/src/roboticstoolbox/robot/DHRobot.py @@ -1874,6 +1874,30 @@ def removesmall(x): # -------------------------------------------------------------------------- # def ikine_6s(self, T, config, ikfunc): + """ + Analytic inverse kinematics for a robot with a spherical wrist + + .. deprecated:: 1.5.0 + This came from the MATLAB Toolbox and is not general. It only handles + the wrist, the user supplied ``ikfunc`` must solve the first three + joints, and that is specific to one robot geometry. It also + mishandles trajectories and unreachable poses. It is no longer used by + :meth:`Puma560.ikine_a`, which is self-contained. A hand-written + solution belongs in the class of the robot it is written for. + + :param T: end-effector pose + :param config: arm configuration string + :param ikfunc: function ``ikfunc(robot, T, config)`` which returns the + joint angles of the first three joints + :returns: an IKSolution + """ + warnings.warn( + "ikine_6s is deprecated and will be removed in a future release, " + "write a hand-written solution for your robot, as Puma560.ikine_a does", + DeprecationWarning, + stacklevel=2, + ) + # Undo base and tool transformations, but if they are not # set, skip the operation. Nicer for symbolics if np.array_equal(self.base.A, np.eye(4)): diff --git a/tests/test_DHRobot.py b/tests/test_DHRobot.py index 0b0a00f6f..b2c010184 100644 --- a/tests/test_DHRobot.py +++ b/tests/test_DHRobot.py @@ -1088,6 +1088,111 @@ def test_ikine_a_with_tool(self): self.assertTrue(sol.success) self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=6) + def _puma_poses(self, puma): + qs = np.array( + [puma.qn, puma.qn + 0.1, [0.3, 0.5, -0.4, 0.2, 0.6, 0.7]], dtype=float + ) + return qs, puma.fkine(qs) + + def test_ikine_a_trajectory(self): + # a trajectory gives one IKSolution like the numerical solvers + puma = rp.models.DH.Puma560() + qs, Ts = self._puma_poses(puma) + + sol = puma.ikine_a(Ts) + + self.assertIsInstance(sol, rp.IKSolution) + self.assertEqual(sol.q.shape, (3, 6)) + self.assertIs(type(sol.success), bool) + self.assertTrue(sol.success) + self.assertEqual(sol.reason, "") + self.assertEqual((sol.iterations, sol.searches), (0, 0)) + self.assertLess(sol.residual, 1e-12) + for k in range(3): + nt.assert_allclose(puma.fkine(sol.q[k]).A, Ts[k].A, atol=1e-9) + + def test_ikine_a_accepts_arrays(self): + puma = rp.models.DH.Puma560() + qs, Ts = self._puma_poses(puma) + + sol_se3 = puma.ikine_a(Ts) + sol_arr = puma.ikine_a(np.array(Ts.A)) # (3, 4, 4) + nt.assert_allclose(sol_arr.q, sol_se3.q) + + sol_one = puma.ikine_a(Ts[0].A) # (4, 4) + self.assertEqual(sol_one.q.shape, (6,)) + nt.assert_allclose(sol_one.q, sol_se3.q[0]) + + for bad in (np.eye(3), np.zeros((2, 3, 4)), np.zeros(16)): + with self.assertRaises(ValueError): + puma.ikine_a(bad) + + def test_ikine_a_unreachable(self): + puma = rp.models.DH.Puma560() + + # far outside the workspace + sol = puma.ikine_a(sm.SE3(5, 5, 5)) + self.assertFalse(sol.success) + self.assertEqual(sol.reason, "Out of reach") + self.assertEqual(sol.q.shape, (6,)) + self.assertTrue(np.all(np.isnan(sol.q))) + self.assertEqual(sol.residual, np.inf) + + # closer to the waist axis than the shoulder offset, this used to + # return success=True with q all NaN + sol = puma.ikine_a(sm.SE3(0.05, 0.0, 0.5) * sm.SE3.Rx(np.pi)) + self.assertFalse(sol.success) + self.assertEqual(sol.reason, "Out of reach") + self.assertTrue(np.all(np.isnan(sol.q))) + + # on the waist axis + sol = puma.ikine_a(sm.SE3(0.0, 0.0, 0.5)) + self.assertFalse(sol.success) + + def test_ikine_a_trajectory_with_unreachable_pose(self): + # this used to raise ValueError + puma = rp.models.DH.Puma560() + qs, Ts = self._puma_poses(puma) + Tmixed = sm.SE3([Ts[0], sm.SE3(5, 5, 5), Ts[2]]) + + sol = puma.ikine_a(Tmixed) + + self.assertEqual(sol.q.shape, (3, 6)) + self.assertIs(type(sol.success), bool) + self.assertFalse(sol.success) + self.assertEqual(sol.reason, "Out of reach") + self.assertEqual(sol.residual, np.inf) + self.assertTrue(np.all(np.isfinite(sol.q[0]))) + self.assertTrue(np.all(np.isnan(sol.q[1]))) + self.assertTrue(np.all(np.isfinite(sol.q[2]))) + nt.assert_allclose(puma.fkine(sol.q[0]).A, Ts[0].A, atol=1e-9) + nt.assert_allclose(puma.fkine(sol.q[2]).A, Ts[2].A, atol=1e-9) + + def test_ikine_a_wrist_singularity(self): + # q5 = 0 aligns the axes of joints 4 and 6 + puma = rp.models.DH.Puma560() + T = puma.fkine([0.3, 0.5, -0.4, 0.2, 0.0, 0.7]) + + sol = puma.ikine_a(T) + + self.assertTrue(sol.success) + nt.assert_allclose(puma.fkine(sol.q).A, T.A, atol=1e-9) + + def test_ikine_a_with_base(self): + puma = rp.models.DH.Puma560() + puma.base = sm.SE3(0.2, -0.1, 0.3) * sm.SE3.Rz(0.4) + T = puma.fkine(puma.qn) + + sol = puma.ikine_a(T) + + self.assertTrue(sol.success) + nt.assert_allclose(puma.fkine(sol.q).A, T.A, atol=1e-9) + + def test_ikine_6s_is_deprecated(self): + puma = rp.models.DH.Puma560() + with self.assertWarns(DeprecationWarning): + puma.ikine_6s(puma.fkine(puma.qn), "lun", lambda robot, T, config: None) + def test_ikine_LM(self): puma = rp.models.DH.Puma560()