diff --git a/src/roboticstoolbox/robot/RobotKinematics.py b/src/roboticstoolbox/robot/RobotKinematics.py index 4e93d7d86..96aedc9f6 100644 --- a/src/roboticstoolbox/robot/RobotKinematics.py +++ b/src/roboticstoolbox/robot/RobotKinematics.py @@ -2,6 +2,7 @@ @author: Jesse Haviland """ +import numpy as np from roboticstoolbox.robot.RobotProto import KinematicsProtocol from roboticstoolbox.tools.types import ArrayLike, NDArray from roboticstoolbox.robot.Link import Link @@ -11,6 +12,30 @@ from typing import Literal as L, overload +def _as_se3(T: NDArray | SE3) -> SE3: + """ + Convert a pose or pose trajectory to an SE3 object + + :param T: an SE3, a 4x4 array, or an (N, 4, 4) array of N poses + :raises ValueError: if ``T`` is an array of any other shape + :returns: an SE3 object holding one or N poses + + .. note:: This is a workaround for a spatialmath (SMTB) limitation. The + ``SE3`` constructor does not accept an (N, 4, 4) array, and with + ``check=False`` it silently builds a malformed object from one. An + (N, 4, 4) array is therefore converted to a list of matrices first. + Once SMTB accepts such arrays this can go, 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") + if T.ndim == 3: + # SMTB workaround, see the note above + return SE3(list(T), check=False) + return SE3(T, check=False) + + class RobotKinematicsMixin: """ The Robot Kinematics Mixin class @@ -680,7 +705,7 @@ def ik_LM( """ return self.ets(start, end).ik_LM( - Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(), + Tep=_as_se3(Tep) * self._resolve_tool(tool).inv(), q0=q0, ilimit=ilimit, slimit=slimit, @@ -805,7 +830,7 @@ def ik_NR( """ return self.ets(start, end).ik_NR( - Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(), + Tep=_as_se3(Tep) * self._resolve_tool(tool).inv(), q0=q0, ilimit=ilimit, slimit=slimit, @@ -945,7 +970,7 @@ def ik_GN( """ return self.ets(start, end).ik_GN( - Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(), + Tep=_as_se3(Tep) * self._resolve_tool(tool).inv(), q0=q0, ilimit=ilimit, slimit=slimit, @@ -1127,7 +1152,7 @@ def ikine_LM( """ return self.ets(start, end).ikine_LM( - Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(), + Tep=_as_se3(Tep) * self._resolve_tool(tool).inv(), q0=q0, ilimit=ilimit, slimit=slimit, @@ -1257,7 +1282,7 @@ def ikine_NR( """ return self.ets(start, end).ikine_NR( - Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(), + Tep=_as_se3(Tep) * self._resolve_tool(tool).inv(), q0=q0, ilimit=ilimit, slimit=slimit, @@ -1401,7 +1426,7 @@ def ikine_GN( """ return self.ets(start, end).ikine_GN( - Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(), + Tep=_as_se3(Tep) * self._resolve_tool(tool).inv(), q0=q0, ilimit=ilimit, slimit=slimit, @@ -1584,7 +1609,7 @@ def ikine_QP( """ return self.ets(start, end).ikine_QP( - Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(), + Tep=_as_se3(Tep) * self._resolve_tool(tool).inv(), q0=q0, ilimit=ilimit, slimit=slimit, diff --git a/tests/test_IK.py b/tests/test_IK.py index 1672ec4f5..a3c533f1a 100644 --- a/tests/test_IK.py +++ b/tests/test_IK.py @@ -623,6 +623,32 @@ def test_ets_ikine_QP2(self): self.assertGreater(test_tol, E) + def test_robot_ikine_accepts_pose_arrays(self): + # Robot.ikine_* must treat an (N, 4, 4) array like the ETS methods do + panda = rtb.models.Panda() + Ts = panda.fkine(np.array([panda.qr, panda.qr + 0.1, panda.qr - 0.1])) + self.assertEqual(len(Ts), 3) + Tarr = np.array(Ts.A) # Ts.A is a list of matrices, make a real 3-D array + self.assertEqual(Tarr.shape, (3, 4, 4)) + + for method in ("ikine_LM", "ikine_GN", "ikine_NR"): + sol = getattr(panda, method)(Tarr, q0=panda.qr, seed=0, joint_limits=False) + self.assertEqual(sol.q.shape, (3, panda.n), method) + sol_se3 = getattr(panda, method)( + Ts, q0=panda.qr, seed=0, joint_limits=False + ) + nt.assert_allclose(sol.q, sol_se3.q, err_msg=method) + + # a single pose as a 4x4 array still works + sol = panda.ikine_LM(Ts[0].A, q0=panda.qr, seed=0) + self.assertEqual(sol.q.shape, (panda.n,)) + + def test_robot_ikine_rejects_badly_shaped_arrays(self): + panda = rtb.models.Panda() + for bad in (np.eye(3), np.zeros((2, 3, 4)), np.zeros(16)): + with self.assertRaises(ValueError): + panda.ikine_LM(bad, q0=panda.qr) + def test_ik_nr(self): tol = 1e-6