fix(puma): self-contained ikine_a with trajectory and failure handling; deprecate ikine_6s

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

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)):

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()