fix(ik): trajectory residual is the maximum, and document trajectory behaviour

docs/source/IK/ik.rst

@@ -24,6 +24,8 @@ A note on the semantics of the above variable:
 
 Therefore, ``Tep`` refers to the desired end-effector pose in the base robot frame represented as an SE(3).
 
+The Python solvers (the methods starting with ``ikine_``) also accept a **trajectory** of poses: an :py:class:`~spatialmath.pose3d.SE3` object containing N poses, or an array with shape (N, 4, 4).  The C++ solvers (the methods starting with ``ik_``) accept a single pose only.
+
 .. rubric:: ilimit
 
 The ``ilimit`` specifies how many iterations are allowed within a single search. After ``ilimit`` is reached, either, a new attempt is made or the IK solution has failed depending on ``slimit``
@@ -79,7 +81,7 @@ These solvers are written in high performance C++ and wrapped in Python methods.
 
 These methods have been written purely for speed so they do not contain the niceties of the Python alternative. For example, if you give the incorrect length for the ``q0`` vector, you could end up with a ``seg-fault`` or other undetermined behaviour. Therefore, when using these methods it is very important that you understand each of the parameters and the parameters passed are of the correct type and length.
 
-The C++ solvers return a tuple with the following members:
+The C++ solvers return an :py:class:`~roboticstoolbox.robot.IK.IKSolution` with the following members (``reason`` is always empty, these solvers do not produce a failure reason string):
 
 ==============   =========   =====================================================================================================
 Element          Type        Description                                                                                                    
@@ -93,6 +95,8 @@ Element          Type        Description
 
 The C++ solvers can be identified as methods which start with ``ik_``.
 
+These solvers handle a single pose only, ``Tep`` must not be a trajectory.  To solve for a trajectory use the Python solvers.
+
 .. rubric:: ETS C++ IK Methods
 
 .. autosummary::
@@ -175,6 +179,19 @@ Element          Type        Description
 ``reason``       `str`       The reason the IK problem failed if applicable   
 ==============   =========   =====================================================================================================
 
+.. rubric:: Solving for a trajectory
+
+If ``Tep`` is a trajectory of N poses, each pose is solved independently, starting from the same ``q0`` (and the same random restarts) for every pose.  Consecutive solutions are therefore not guaranteed to be on the same IK branch.  The members of the returned :py:class:`~roboticstoolbox.robot.IK.IKSolution` are:
+
+* ``q`` has shape (N, n), one row per pose
+* ``success`` is True only if every pose was solved
+* ``iterations`` and ``searches`` are summed over all poses
+* ``residual`` is the maximum over all poses, the worst case
+* ``reason`` is the reason given by the last pose that failed, if any
+
+.. versionchanged:: 1.5.0
+    For a trajectory, ``residual`` is the maximum over the poses, it was the minimum.
+
 .. rubric:: The Implemented IK Solvers
 
 These solvers can be identified as a :py:class:`Class` starting with ``IK_``.

src/roboticstoolbox/ets/ETS.py

@@ -1169,7 +1169,7 @@ def ik_NR(
         r"""
         Fast numerical inverse kinematics using Newton-Raphson optimisation
 
-        :param Tep: the desired end-effector pose or pose trajectory
+        :param Tep: the desired end-effector pose (a single pose only, for a trajectory use :meth:`ikine_NR`)
         :param q0: initial joint configuration (random valid configuration if not supplied)
         :param ilimit: maximum number of iterations per search
         :param slimit: maximum number of search attempts
@@ -1275,7 +1275,7 @@ def ik_GN(
         r"""
         Fast numerical inverse kinematics by Gauss-Newton optimisation
 
-        :param Tep: the desired end-effector pose or pose trajectory
+        :param Tep: the desired end-effector pose (a single pose only, for a trajectory use :meth:`ikine_GN`)
         :param q0: initial joint configuration (random valid configuration if not supplied)
         :param ilimit: maximum number of iterations per search
         :param slimit: maximum number of search attempts
@@ -1284,8 +1284,11 @@ def ik_GN(
         :param joint_limits: reject solutions with invalid joint configurations
         :param pinv: use the pseudo-inverse instead of the normal matrix inverse
         :param pinv_damping: damping factor for the pseudo-inverse
-        :returns: tuple (q, success, iterations, searches, residual)
-        :rtype: tuple
+        :returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
+            ``iterations``, ``searches`` and ``residual`` error value (``reason`` is
+            always empty -- this fast C++ solver doesn't produce a granular failure
+            reason string, unlike :meth:`ikine_GN`)
+        :rtype: IKSolution
 
         ``sol = ets.ik_GN(Tep)`` are the joint coordinates (n) corresponding
         to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object.
@@ -1392,8 +1395,10 @@ def ikine_LM(
         r"""
         Levenberg-Marquardt numerical inverse kinematics solver
 
-        :param Tep: the desired end-effector pose
-        :param q0: the initial joint coordinate vector
+        :param Tep: the desired end-effector pose or pose trajectory
+        :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 valid coordinates)
         :param ilimit: maximum iterations allowed per search
         :param slimit: maximum search attempts before failure
         :param tol: maximum allowed residual error E
@@ -1411,6 +1416,16 @@ def ikine_LM(
             string if applicable
         :rtype: IKSolution
 
+        If ``Tep`` is a trajectory (an :class:`SE3` with N poses, or an array with
+        shape (N, 4, 4)) each pose is solved independently from the same ``q0``, so
+        consecutive solutions are not guaranteed to be on the same IK branch.  ``q``
+        has shape (N, n), ``success`` is True only if every pose succeeded, and
+        ``residual`` is the maximum over the poses.  See :meth:`IKSolver.solve`.
+
+        .. versionchanged:: 1.5.0
+            For a trajectory, ``residual`` is the maximum over the poses, it was
+            the minimum.
+
         A method which provides functionality to perform numerical inverse kinematics (IK)
         using the Levenberg-Marquardt method.
 
@@ -1555,8 +1570,10 @@ def ikine_NR(
         r"""
         Newton-Raphson numerical inverse kinematics solver
 
-        :param Tep: the desired end-effector pose
-        :param q0: the initial joint coordinate vector
+        :param Tep: the desired end-effector pose or pose trajectory
+        :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 valid coordinates)
         :param ilimit: maximum iterations allowed per search
         :param slimit: maximum search attempts before failure
         :param tol: maximum allowed residual error E
@@ -1573,6 +1590,16 @@ def ikine_NR(
             string if applicable
         :rtype: IKSolution
 
+        If ``Tep`` is a trajectory (an :class:`SE3` with N poses, or an array with
+        shape (N, 4, 4)) each pose is solved independently from the same ``q0``, so
+        consecutive solutions are not guaranteed to be on the same IK branch.  ``q``
+        has shape (N, n), ``success`` is True only if every pose succeeded, and
+        ``residual`` is the maximum over the poses.  See :meth:`IKSolver.solve`.
+
+        .. versionchanged:: 1.5.0
+            For a trajectory, ``residual`` is the maximum over the poses, it was
+            the minimum.
+
         A method which provides functionality to perform numerical inverse kinematics (IK)
         using the Newton-Raphson method.
 
@@ -1662,8 +1689,10 @@ def ikine_GN(
         r"""
         Gauss-Newton numerical inverse kinematics solver
 
-        :param Tep: the desired end-effector pose
-        :param q0: the initial joint coordinate vector
+        :param Tep: the desired end-effector pose or pose trajectory
+        :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 valid coordinates)
         :param ilimit: maximum iterations allowed per search
         :param slimit: maximum search attempts before failure
         :param tol: maximum allowed residual error E
@@ -1680,6 +1709,16 @@ def ikine_GN(
             string if applicable
         :rtype: IKSolution
 
+        If ``Tep`` is a trajectory (an :class:`SE3` with N poses, or an array with
+        shape (N, 4, 4)) each pose is solved independently from the same ``q0``, so
+        consecutive solutions are not guaranteed to be on the same IK branch.  ``q``
+        has shape (N, n), ``success`` is True only if every pose succeeded, and
+        ``residual`` is the maximum over the poses.  See :meth:`IKSolver.solve`.
+
+        .. versionchanged:: 1.5.0
+            For a trajectory, ``residual`` is the maximum over the poses, it was
+            the minimum.
+
         A method which provides functionality to perform numerical inverse kinematics (IK)
         using the Gauss-Newton method.
 
@@ -1785,8 +1824,10 @@ def ikine_QP(
         r"""
         Quadratic programming numerical inverse kinematics solver
 
-        :param Tep: the desired end-effector pose
-        :param q0: the initial joint coordinate vector
+        :param Tep: the desired end-effector pose or pose trajectory
+        :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 valid coordinates)
         :param ilimit: maximum iterations allowed per search
         :param slimit: maximum search attempts before failure
         :param tol: maximum allowed residual error E
@@ -1803,6 +1844,16 @@ def ikine_QP(
             ``iterations``, ``searches``, ``residual`` error value, and ``reason``
             string if applicable
         :rtype: IKSolution
+
+        If ``Tep`` is a trajectory (an :class:`SE3` with N poses, or an array with
+        shape (N, 4, 4)) each pose is solved independently from the same ``q0``, so
+        consecutive solutions are not guaranteed to be on the same IK branch.  ``q``
+        has shape (N, n), ``success`` is True only if every pose succeeded, and
+        ``residual`` is the maximum over the poses.  See :meth:`IKSolver.solve`.
+
+        .. versionchanged:: 1.5.0
+            For a trajectory, ``residual`` is the maximum over the poses, it was
+            the minimum.
         :raises ImportError: if the package ``qpsolvers`` is not installed
 
         A method that provides functionality to perform numerical inverse kinematics

src/roboticstoolbox/robot/DHRobot.py

@@ -2005,8 +2005,11 @@ def ikine_LM(
         """
         Numerical inverse kinematics by Levenberg-Marquadt optimization
 
-        :param Tep: The desired end-effector pose
-        :param q0: The initial joint coordinate vector
+        :param Tep: The desired end-effector pose or pose trajectory, see
+            :meth:`ETS.ikine_LM` for how a trajectory is handled
+        :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 valid coordinates)
         :param ikargs: |ikargs|
         :returns: An IKSolution containing joint coordinates ``q``, ``success`` flag, ``iterations``, ``searches``, ``residual`` error value, and ``reason`` string if applicable
         :rtype: IKSolution

src/roboticstoolbox/robot/IK.py

@@ -191,14 +191,32 @@ def solve(
 
         :param ets: The ETS representing the manipulators kinematics
         :param Tep: The desired end-effector pose
-        :param q0: The initial joint coordinate vector
+        :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 valid coordinates)
         :returns: An IKSolution containing joint coordinates ``q``, ``success`` flag,
             ``iterations``, ``searches``, ``residual`` error value, and ``reason``
             string if applicable
         :rtype: IKSolution
 
         This method will attempt to solve the IK problem and obtain joint coordinates
         which result the the end-effector pose `Tep`.
+
+        If `Tep` is an :class:`SE3` containing more than one pose, or an array with
+        shape (N, 4, 4), it is treated as a trajectory of N poses.  Each pose is
+        solved independently, using the same initial coordinates `q0` (and the same
+        random restarts) for every pose, so consecutive solutions are not guaranteed
+        to lie on the same IK branch.  The returned :class:`IKSolution` has:
+
+        - ``q`` with shape (N, n), one row per pose
+        - ``success`` True only if *every* pose was solved
+        - ``iterations`` and ``searches`` summed over all poses
+        - ``residual`` the *maximum* residual over all poses, the worst case
+        - ``reason`` the reason given by the last pose that failed, if any
+
+        .. versionchanged:: 1.5.0
+            For a trajectory, ``residual`` is now the maximum over the poses,
+            previously it was the minimum, which hid poses that were solved badly.
         """
         # Get the largest jindex in the ETS. If this is greater than ETS.n
         # then we need to pad the q vector with zeros
@@ -245,7 +263,7 @@ def solve(
             traj = True
             methTep = Tep
         elif Tep.shape != (4, 4):
-            raise ValueError("Tep must be a 4x4 SE3 matrix")
+            raise ValueError("Tep must be an SE3, a 4x4 array or an (N, 4, 4) array")
         else:
             methTep = Tep
 
@@ -254,7 +272,7 @@ def solve(
             success = True
             interations = 0
             searches = 0
-            residual = np.inf
+            residual = 0.0
             reason = ""
 
             for i, T in enumerate(methTep):
@@ -266,7 +284,7 @@ def solve(
                 interations += sol.iterations
                 searches += sol.searches
 
-                if sol.residual < residual:
+                if sol.residual > residual:
                     residual = sol.residual
 
             return IKSolution(