diff --git a/README.md b/README.md index 632556a91..756a461c6 100644 --- a/README.md +++ b/README.md @@ -264,16 +264,15 @@ Tep = SE3.Trans(0.6, -0.3, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) sol = robot.ik_LM(Tep) # solve IK print(sol) - (array([ 0.20592815, 0.86609481, -0.79473206, -1.68254794, 0.74872915, - 2.21764746, -0.10255606]), 1, 114, 7, 2.890164057230228e-07) + IKSolution: q=[-2.709, -1.176, 1.961, -1.642, 1.173, 1.958, 2.876], success=True, iterations=6, searches=1, residual=1.06e-07 -q_pickup = sol[0] +q_pickup = sol.q print(robot.fkine(q_pickup)) # FK shows that desired end-effector pose was achieved - 1 -8.913e-05 -0.0003334 0.5996 - -8.929e-05 -1 -0.0004912 -0.2998 - -0.0003334 0.0004912 -1 0.1001 - 0 0 0 1 + -1 0.0002931 0.0003122 0.5999 + 0.0002931 1 7.601e-05 -0.2999 + -0.0003121 7.611e-05 -1 0.1001 + 0 0 0 1 ``` We can animate a path from the ready pose `qr` configuration to this pickup configuration diff --git a/docs/source/IK/ik.rst b/docs/source/IK/ik.rst index 8e934b8af..e42ac8170 100644 --- a/docs/source/IK/ik.rst +++ b/docs/source/IK/ik.rst @@ -162,7 +162,7 @@ The :py:class:`~roboticstoolbox.robot.IK.IKSolver` provides basic functionality iksolution -The :py:class:`~roboticstoolbox.robot.IK.IKSolution` is a :py:class:`dataclasses.dataclass` instance with the following members. +The :py:class:`~roboticstoolbox.robot.IK.IKSolution` is a :py:class:`dataclasses.dataclass` instance with the following members. It is also a sequence of the rows of ``q``, one per pose: ``len(sol)`` is the number of poses, ``for q in sol`` and ``sol[i]`` give the joint coordinates of each pose, and ``bool(sol)`` is ``success``. Iterating over the six members was removed in 1.5.0, use the attributes or ``sol.astuple()``. ============== ========= ===================================================================================================== Element Type Description diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index 232eb74cf..ff5b8a531 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/src/roboticstoolbox/robot/IK.py @@ -30,7 +30,8 @@ class IKSolution: ---------- q The joint coordinates of the solution (ndarray). Note that these - will not be valid if failed to find a solution + will not be valid if failed to find a solution. The shape is (n,) for a + single pose, or (N, n) for a trajectory of N poses success True if a valid solution was found iterations @@ -52,6 +53,33 @@ class IKSolution: ``ik_GN`` (which now also return ``IKSolution``, see :meth:`ETS.ik_LM`) remain compatible with code that indexed the old bare tuple return + .. versionchanged:: 1.5.0 + An ``IKSolution`` is now a sequence of the rows of ``q``, one per pose: + ``len(sol)`` is the number of poses (1 for a single pose), and + ``for q in sol`` and ``sol[i]`` give the joint vector of each pose. This + replaces iterating over the fields, so ``q, success, ... = sol`` and + ``sol[1]`` no longer work, use the attributes or :meth:`astuple`. An + ``IKSolution`` is true if ``success`` is true. Printing a trajectory + shows the number of poses and abbreviates a long ``q``, and the + residual of a failed solution is no longer rounded to zero. + + The solution is a sequence of the rows of ``q``, one per pose. + + .. runblock:: pycon + >>> import numpy as np + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> T = panda.fkine(panda.qr + np.array([[0], [0.05], [0.1]])) + >>> sol = panda.ikine_LM(T, q0=panda.qr, seed=0) + >>> len(sol) + >>> sol[2] + >>> sol[:2] + >>> bool(sol) == sol.success + + A single pose is treated as one row, so ``len(sol)`` is 1 and ``sol[0]`` is + ``q``. ``sol[i]`` is the same as ``np.atleast_2d(sol.q)[i]``, it does not + return an ``IKSolution`` since ``success``, ``residual`` and ``reason`` are + values for the whole solution, not for each pose. """ q: np.ndarray @@ -61,56 +89,113 @@ class IKSolution: residual: float = 0.0 reason: str = "" + def _rows(self) -> NDArray: + # the joint coordinates as an array with one row per pose + if self.q is None: + return np.empty((0, 0)) + return np.atleast_2d(self.q) + + def __len__(self) -> int: + """ + Number of poses, the number of rows of ``q`` + + :returns: 1 for a single pose, N for a trajectory of N poses, 0 if ``q`` is + ``None`` + """ + return 0 if self.q is None else self._rows().shape[0] + def __iter__(self): - return iter( - ( - self.q, - self.success, - self.iterations, - self.searches, - self.residual, - self.reason, - ) - ) + """ + Iterate over the joint coordinates of each pose + + :returns: an iterator over ``q``, one ndarray(n) per pose + """ + return iter(self._rows()) def __getitem__(self, i): - return tuple(self)[i] + """ + Joint coordinates of one or more poses + + :param i: an integer, slice or index array, as for a NumPy array + :returns: ``np.atleast_2d(self.q)[i]``, an ndarray(n) for an integer index + :raises IndexError: if the index is out of range + """ + return self._rows()[i] + + def __bool__(self) -> bool: + """ + True if the IK solution was successful + + :returns: the value of ``success`` + """ + return bool(self.success) + + def astuple(self) -> tuple: + """ + The fields of the solution as a tuple + + :returns: ``(q, success, iterations, searches, residual, reason)`` + + This is what iterating over an ``IKSolution`` gave before 1.5.0. + """ + return ( + self.q, + self.success, + self.iterations, + self.searches, + self.residual, + self.reason, + ) def __repr__(self): return str(self) + def _q_str(self) -> str | None: + # joint coordinates as text, a trajectory of many poses is abbreviated + if self.q is None: + return None + + fmt = {"float": lambda x: "{:.4g}".format(0 if abs(x) < 1e-6 else x)} + + def text(a): + return np.array2string(a, separator=", ", formatter=fmt) + + if self.q.ndim == 1 or len(self) <= 6: + return text(self.q) + + # first three poses, an ellipsis, then the last two + rows = self._rows() + lines = [text(r) for r in rows[:3]] + ["..."] + [text(r) for r in rows[-2:]] + return "[" + ",\n ".join(lines) + "]" + def __str__(self): - if self.q is not None: - q_str = np.array2string( - self.q, - separator=", ", - formatter={ - "float": lambda x: "{:.4g}".format(0 if abs(x) < 1e-6 else x) - }, - ) # np.round(self.q, 4) - else: - q_str = None + analytic = self.iterations == 0 and self.searches == 0 - if self.iterations == 0 and self.searches == 0: - # Check for analytic - if self.success: - return f"IKSolution: q={q_str}, success=True" - else: - return f"IKSolution: q={q_str}, success=False, reason={self.reason}" + # everything after q + if self.success: + status = "success=True" + else: + status = f"success=False, reason={self.reason}" + if analytic: + # not iterative, show the residual if one was computed + if self.residual != 0: + status += f", residual={self.residual:.3g}" else: - # Otherwise it is a numeric solution - if self.success: - return ( - f"IKSolution: q={q_str}, success=True," + status += ( + f", iterations={self.iterations}, searches={self.searches}," + f" residual={self.residual:.3g}" + ) + if not self.success: + # reason comes before the counts for a failure + status = ( + f"success=False, reason={self.reason}," f" iterations={self.iterations}, searches={self.searches}," f" residual={self.residual:.3g}" ) - else: - return ( - f"IKSolution: q={q_str}, success=False, reason={self.reason}," - f" iterations={self.iterations}, searches={self.searches}," - f" residual={np.round(self.residual, 4):.3g}" - ) + + if self.q is not None and self.q.ndim == 2 and len(self) > 1: + return f"IKSolution: {len(self)} poses, {status}\nq={self._q_str()}" + return f"IKSolution: q={self._q_str()}, {status}" class IKSolver(ABC): @@ -1437,6 +1522,6 @@ def step( np.array([1, 2, 3]), success=True, iterations=10, searches=100, residual=0.1 ) - a, b, c, d, e = sol + a, b, c, d, e, f = sol.astuple() - print(a, b, c, d, e) + print(a, b, c, d, e, f) diff --git a/tests/test_IK.py b/tests/test_IK.py index 1672ec4f5..30c6943e7 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 @@ -637,11 +638,11 @@ def test_ik_nr(self): sol = r.ik_NR(Tep, tol=tol) sol2 = r2.ik_NR(Tep, tol=tol) - self.assertEqual(sol[1], True) - self.assertEqual(sol2[1], True) + self.assertTrue(sol.success) + self.assertTrue(sol2.success) - Tq = r.eval(sol[0]) - Tq2 = r.eval(sol2[0]) + Tq = r.eval(sol.q) + Tq2 = r.eval(sol2.q) _, E = solver.error(Tep, Tq) _, E2 = solver.error(Tep, Tq2) @@ -663,11 +664,11 @@ def test_ik_lm_chan(self): sol = r.ik_LM(Tep, tol=tol, method="chan") sol2 = r2.ik_LM(Tep, tol=tol, method="chan") - self.assertEqual(sol[1], True) - self.assertEqual(sol2[1], True) + self.assertTrue(sol.success) + self.assertTrue(sol2.success) - Tq = r.eval(sol[0]) - Tq2 = r.eval(sol2[0]) + Tq = r.eval(sol.q) + Tq2 = r.eval(sol2.q) _, E = solver.error(Tep, Tq) _, E2 = solver.error(Tep, Tq2) @@ -689,11 +690,11 @@ def test_ik_lm_wampler(self): sol = r.ik_LM(Tep, tol=tol, method="wampler", k=0.01) sol2 = r2.ik_LM(Tep, tol=tol, method="wampler", k=0.01) - self.assertEqual(sol[1], True) - self.assertEqual(sol2[1], True) + self.assertTrue(sol.success) + self.assertTrue(sol2.success) - Tq = r.eval(sol[0]) - Tq2 = r.eval(sol2[0]) + Tq = r.eval(sol.q) + Tq2 = r.eval(sol2.q) _, E = solver.error(Tep, Tq) _, E2 = solver.error(Tep, Tq2) @@ -715,11 +716,11 @@ def test_ik_lm_sugihara(self): sol = r.ik_LM(Tep, tol=tol, k=0.01, method="sugihara") sol2 = r2.ik_LM(Tep, tol=tol, k=0.01, method="sugihara") - self.assertEqual(sol[1], True) - self.assertEqual(sol2[1], True) + self.assertTrue(sol.success) + self.assertTrue(sol2.success) - Tq = r.eval(sol[0]) - Tq2 = r.eval(sol2[0]) + Tq = r.eval(sol.q) + Tq2 = r.eval(sol2.q) _, E = solver.error(Tep, Tq) _, E2 = solver.error(Tep, Tq2) @@ -741,13 +742,13 @@ def test_ik_gn(self): sol = r.ik_GN(Tep, tol=tol) sol2 = r2.ik_GN(Tep, tol=tol) - self.assertEqual(sol[1], True) - self.assertEqual(sol2[1], True) + self.assertTrue(sol.success) + self.assertTrue(sol2.success) - Tq = r.eval(sol[0]) - Tq2 = r.eval(sol2[0]) + Tq = r.eval(sol.q) + Tq2 = r.eval(sol2.q) - print(sol[4]) + print(sol.residual) print(Tep) print(Tq) @@ -807,7 +808,7 @@ def test_sol_print3(self): s = sol.__str__() - ans = "IKSolution: q=[0, 0, 0], success=False, reason=no" + ans = "IKSolution: q=[0, 0, 0], success=False, reason=no, residual=3" self.assertEqual(s, ans) @@ -824,7 +825,7 @@ def test_sol_print4(self): s = sol.__str__() - ans = "IKSolution: q=[0, 0, 0], success=True" + ans = "IKSolution: q=[0, 0, 0], success=True, residual=3" self.assertEqual(s, ans) @@ -848,7 +849,56 @@ def test_sol_print5(self): self.assertEqual(s, ans) - def test_getitem_iksol(self): + def test_iksol_single_pose_is_one_row(self): + q = np.array([1.0, 2.0, 3.0]) + sol = rtb.IKSolution(q, success=True) + + self.assertEqual(len(sol), 1) + nt.assert_array_equal(sol[0], q) + nt.assert_array_equal(sol[-1], q) + rows = list(sol) + self.assertEqual(len(rows), 1) + nt.assert_array_equal(rows[0], q) + with self.assertRaises(IndexError): + sol[1] + + def test_iksol_trajectory_is_a_sequence_of_rows(self): + q = np.arange(12.0).reshape(4, 3) + sol = rtb.IKSolution(q, success=True) + + self.assertEqual(len(sol), 4) + nt.assert_array_equal(sol[0], q[0]) + nt.assert_array_equal(sol[2], q[2]) + nt.assert_array_equal(sol[-1], q[-1]) + nt.assert_array_equal(sol[1:3], q[1:3]) + nt.assert_array_equal(sol[[0, 3]], q[[0, 3]]) + for k, row in enumerate(sol): + nt.assert_array_equal(row, q[k]) + with self.assertRaises(IndexError): + sol[4] + + # the sequence protocol means NumPy sees the rows + nt.assert_array_equal(np.array(sol), q) + nt.assert_array_equal(np.array(rtb.IKSolution(q[0], success=True)), q[:1]) + + def test_iksol_without_q_is_empty(self): + sol = rtb.IKSolution(None, success=False) # type: ignore + + self.assertEqual(len(sol), 0) + self.assertEqual(list(sol), []) + with self.assertRaises(IndexError): + sol[0] + + def test_iksol_bool_is_success(self): + q = np.zeros((2, 3)) + + self.assertTrue(rtb.IKSolution(q, success=True)) + # not the same as having rows + self.assertFalse(rtb.IKSolution(q, success=False)) + self.assertFalse(rtb.IKSolution(None, success=False)) # type: ignore + self.assertFalse(rtb.IKSolution(np.zeros(3), success=False)) + + def test_iksol_astuple(self): sol = rtb.IKSolution( np.array([1.0, 2.0, 3.0]), success=True, @@ -858,12 +908,75 @@ def test_getitem_iksol(self): reason="ok", ) - nt.assert_almost_equal(sol[0], np.array([1.0, 2.0, 3.0])) # type: ignore - self.assertEqual(sol[1], True) - self.assertEqual(sol[2], 10) - self.assertEqual(sol[3], 100) - self.assertEqual(sol[4], 0.1) - self.assertEqual(sol[5], "ok") + q, success, iterations, searches, residual, reason = sol.astuple() + + nt.assert_almost_equal(q, np.array([1.0, 2.0, 3.0])) # type: ignore + self.assertEqual(success, True) + self.assertEqual(iterations, 10) + self.assertEqual(searches, 100) + self.assertEqual(residual, 0.1) + self.assertEqual(reason, "ok") + + def test_iksol_from_a_solver_trajectory(self): + panda = rtb.models.Panda().ets() + Tep = panda.eval(np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])) + Teps = SE3([SE3(Tep), SE3(Tep)]) + + sol = rtb.IK_LM(seed=0).solve(panda, Teps) + + self.assertEqual(len(sol), 2) + self.assertEqual(sol[0].shape, (panda.n,)) + self.assertEqual(bool(sol), sol.success) + + def test_sol_print_trajectory(self): + q = np.arange(9.0).reshape(3, 3) + sol = rtb.IKSolution(q, success=True, iterations=4, searches=3, residual=1e-8) + + lines = str(sol).split("\n") + + self.assertEqual( + lines[0], + "IKSolution: 3 poses, success=True, iterations=4, searches=3," + " residual=1e-08", + ) + self.assertEqual(lines[1], "q=[[0, 1, 2],") + self.assertEqual(lines[-1], " [6, 7, 8]]") + self.assertEqual(repr(sol), str(sol)) + + def test_sol_print_long_trajectory_is_abbreviated(self): + q = np.arange(300.0).reshape(100, 3) + sol = rtb.IKSolution(q, success=False, iterations=9, searches=2, reason="no") + + lines = str(sol).split("\n") + + self.assertTrue(lines[0].startswith("IKSolution: 100 poses, success=False,")) + self.assertIn("reason=no", lines[0]) + self.assertLessEqual(len(lines), 8) + self.assertIn("[0, 1, 2]", lines[1]) # first pose + self.assertIn("...", str(sol)) + self.assertIn("[297, 298, 299]", lines[-1]) # last pose + + def test_sol_print_failed_residual_is_not_rounded_to_zero(self): + sol = rtb.IKSolution( + np.zeros(3), + success=False, + iterations=3, + searches=2, + residual=1.5e-5, + reason="no", + ) + + self.assertIn("residual=1.5e-05", str(sol)) + + def test_sol_print_analytic_without_residual(self): + # an analytic solution that did not compute a residual has none to show + sol = rtb.IKSolution(np.zeros(3), success=True) + self.assertEqual(str(sol), "IKSolution: q=[0, 0, 0], success=True") + + sol = rtb.IKSolution(np.zeros(3), success=False, reason="Out of reach") + self.assertEqual( + str(sol), "IKSolution: q=[0, 0, 0], success=False, reason=Out of reach" + ) def test_repr_iksol(self): sol = rtb.IKSolution(np.array([1.0, 2.0, 3.0]), success=True) @@ -912,24 +1025,6 @@ def test_ik_lm_failure_returns_compact_q(self): self.assertFalse(sol.success) self.assertEqual(sol.q.shape[0], ets.n) - def test_iter_iksol(self): - sol = rtb.IKSolution( - np.array([1.0, 2.0, 3.0]), - success=True, - iterations=10, - searches=100, - residual=0.1, - ) - - a, b, c, d, e, f = sol - - nt.assert_almost_equal(a, np.array([1.0, 2.0, 3.0])) # type: ignore - self.assertEqual(b, True) - self.assertEqual(c, 10) - self.assertEqual(d, 100) - self.assertEqual(e, 0.1) - self.assertEqual(f, "") - def test_random_q_rejects_non_finite_qlim(self): # _random_q() used to sample straight from ets.qlim with no check -- # a joint with a bad (non-finite) limit baked into its model data