Skip to content

fix(ik): Robot.ikine_* accept an (N, 4, 4) array of poses #714

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We鈥檒l occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
petercorke wants to merge 1 commit into
base: main
Choose a base branch
from
Open

fix(ik): Robot.ikine_* accept an (N, 4, 4) array of poses #714

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 32 additions & 7 deletions src/roboticstoolbox/robot/RobotKinematics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down
26 changes: 26 additions & 0 deletions tests/test_IK.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading