diff --git a/docs/source/IK/ik.rst b/docs/source/IK/ik.rst index 8e934b8af..f27a6f758 100644 --- a/docs/source/IK/ik.rst +++ b/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_``. diff --git a/src/roboticstoolbox/ets/ETS.py b/src/roboticstoolbox/ets/ETS.py index 583120e90..dfed76421 100644 --- a/src/roboticstoolbox/ets/ETS.py +++ b/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 diff --git a/src/roboticstoolbox/robot/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index 5e2d5f431..b6d1a9ebc 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/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 diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index 232eb74cf..f35c8b4df 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/src/roboticstoolbox/robot/IK.py @@ -191,7 +191,9 @@ 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 @@ -199,6 +201,22 @@ def solve( 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( diff --git a/src/roboticstoolbox/robot/RobotKinematics.py b/src/roboticstoolbox/robot/RobotKinematics.py index 4e93d7d86..9fb24073e 100644 --- a/src/roboticstoolbox/robot/RobotKinematics.py +++ b/src/roboticstoolbox/robot/RobotKinematics.py @@ -709,7 +709,7 @@ def ik_NR( r""" Fast numerical inverse kinematics using Newton-Raphson optimization - :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 end: the link considered as the end-effector :param start: the link considered as the base frame, defaults to the robots's base frame :param q0: initial joint configuration (default to random valid joint @@ -834,7 +834,7 @@ def ik_GN( r""" Fast numerical inverse kinematics by Gauss-Newton optimization - :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 end: the link considered as the end-effector :param start: the link considered as the base frame, defaults to the robots's base frame :param q0: initial joint configuration (default to random valid joint @@ -980,10 +980,12 @@ def ikine_LM( r""" Levenberg-Marquardt Numerical Inverse Kinematics Solver - :param Tep: The desired end-effector pose + :param Tep: The desired end-effector pose or pose trajectory :param end: the link considered as the end-effector :param start: the link considered as the base frame, defaults to the robots's base frame - :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) :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful @@ -1012,6 +1014,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. @@ -1167,10 +1179,12 @@ def ikine_NR( r""" Newton-Raphson Numerical Inverse Kinematics Solver - :param Tep: The desired end-effector pose + :param Tep: The desired end-effector pose or pose trajectory :param end: the link considered as the end-effector :param start: the link considered as the base frame, defaults to the robots's base frame - :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) :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful @@ -1193,6 +1207,20 @@ def ikine_NR( :param tool: a static tool transformation matrix to apply to the end of ``end``; defaults to the robot's own ``self.tool`` if not given + :returns: an IKSolution containing joint coordinates ``q``, ``success`` flag, + ``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. A method which provides functionality to perform numerical inverse kinematics (IK) using the Newton-Raphson method. @@ -1296,10 +1324,12 @@ def ikine_GN( r""" Gauss-Newton Numerical Inverse Kinematics Solver - :param Tep: The desired end-effector pose + :param Tep: The desired end-effector pose or pose trajectory :param end: the link considered as the end-effector :param start: the link considered as the base frame, defaults to the robots's base frame - :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) :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful @@ -1322,6 +1352,20 @@ def ikine_GN( :param tool: a static tool transformation matrix to apply to the end of ``end``; defaults to the robot's own ``self.tool`` if not given + :returns: an IKSolution containing joint coordinates ``q``, ``success`` flag, + ``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. A method which provides functionality to perform numerical inverse kinematics (IK) using the Gauss-Newton method. @@ -1441,10 +1485,12 @@ def ikine_QP( r""" Quadratic Programming Numerical Inverse Kinematics Solver - :param Tep: The desired end-effector pose + :param Tep: The desired end-effector pose or pose trajectory :param end: the link considered as the end-effector :param start: the link considered as the base frame, defaults to the robots's base frame - :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) :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful @@ -1473,6 +1519,16 @@ def ikine_QP( 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 that provides functionality to perform numerical inverse kinematics (IK) using a quadratic programming approach. diff --git a/tests/test_IK.py b/tests/test_IK.py index 1672ec4f5..ffeb56b2b 100644 --- a/tests/test_IK.py +++ b/tests/test_IK.py @@ -10,6 +10,7 @@ # import sympy import pytest +from spatialmath import SE3 from tests import skip_no_qp test_tol = 1e-5 @@ -757,6 +758,62 @@ def test_ik_gn(self): self.assertGreater(test_tol, E) self.assertGreater(test_tol, E2) + def test_trajectory_residual_is_maximum(self): + # for a trajectory, residual is the worst case over the poses, not the best + residuals = iter([0.5, 3.0, 0.25]) + + class Stub(rtb.IKSolver): + def step(self, ets, Tep, q): + raise NotImplementedError + + def _solve(self, ets, Tep, q0): + return rtb.IKSolution( + q=np.zeros(ets.n), + success=True, + iterations=1, + searches=1, + residual=next(residuals), + ) + + panda = rtb.models.Panda().ets() + Teps = np.array([np.eye(4)] * 3) + + sol = Stub().solve(panda, Teps) + + self.assertEqual(sol.q.shape, (3, panda.n)) + self.assertEqual(sol.residual, 3.0) + self.assertEqual(sol.iterations, 3) + self.assertEqual(sol.searches, 3) + + def test_trajectory_failure_is_reported(self): + # one reachable and one unreachable pose: the failure must not be hidden + # by the small residual of the pose that was solved + panda = rtb.models.Panda().ets() + good = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + bad = SE3(5, 5, 5).A + + solver = rtb.IK_LM(seed=0, slimit=5) + sol = solver.solve(panda, SE3([SE3(good), SE3(bad)])) + + self.assertEqual(sol.q.shape, (2, panda.n)) + self.assertFalse(sol.success) + self.assertGreater(sol.residual, solver.tol) + + def test_q0_matrix_seeds_the_first_searches(self): + # row k of an (m, n) q0 is the starting point of search k + panda = rtb.models.Panda().ets() + q_true = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + Tep = panda.eval(q_true) + q0 = np.vstack([np.zeros(panda.n), q_true]) + + # one iteration per search: the first seed cannot converge, the second is exact + solver = rtb.IK_LM(seed=0, ilimit=1, joint_limits=False) + sol = solver.solve(panda, Tep, q0) + + self.assertTrue(sol.success) + self.assertEqual(sol.searches, 2) + nt.assert_allclose(sol.q, q_true, atol=1e-6) + def test_sol_print1(self): sol = rtb.IKSolution(