diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 33878731c..6b07b3257 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1012,40 +1012,57 @@ def asada(robot, J, q, axes_list): def jtraj( self, - T1: NDArray | SE3, - T2: NDArray | SE3, - t: NDArray | int, - **kwargs, + q0: ArrayLike, + qf: ArrayLike, + t: ArrayLike | int, + qd0: ArrayLike | None = None, + qd1: ArrayLike | None = None, ): """ - Joint-space trajectory between SE(3) poses + Joint-space trajectory between two joint configurations - :param T1: initial end-effector pose - :param T2: final end-effector pose + :param q0: initial joint coordinates + :param qf: final joint coordinates :param t: time vector or number of steps - :param kwargs: arguments passed to the IK solver + :param qd0: initial joint velocity, defaults to zero + :param qd1: final joint velocity, defaults to zero + :raises TypeError: if ``q0`` or ``qf`` is an :class:`SE3` :returns: trajectory - The initial and final poses are mapped to joint space using inverse - kinematics: - - - if the object has an analytic solution ``ikine_a`` that will be used, - - otherwise the general numerical algorithm ``ikine_lm`` will be used. - - ``traj = obot.jtraj(T1, T2, t)`` is a trajectory object whose - attribute ``traj.q`` is a row-wise joint-space trajectory. - + ``traj = robot.jtraj(q0, qf, t)`` is a trajectory object whose + attribute ``traj.q`` is a row-wise joint-space trajectory, a quintic + polynomial in time from ``q0`` to ``qf``. It is the method form of + :func:`~roboticstoolbox.tools.trajectory.jtraj`. + + .. deprecated:: 1.5.0 + Passing :class:`SE3` poses for ``q0`` or ``qf`` is deprecated and now + raises ``TypeError``. The poses were converted to joint coordinates + by inverse kinematics, which is never simple: which solution is chosen + depends on the solver and its starting point, and it can fail. Do + that explicitly, so you control and can check it:: + + q0 = robot.ikine_LM(T0, q0=robot.qr).q + qf = robot.ikine_LM(Tf, q0=q0).q + traj = robot.jtraj(q0, qf, t) + + For a Cartesian trajectory see + :func:`~roboticstoolbox.tools.trajectory.ctraj`. + + .. versionchanged:: 1.5.0 + The arguments are joint coordinates only, they were named ``T1`` and + ``T2`` and could be poses. The ``**kwargs`` that were passed to the + inverse kinematics method are gone. """ - if hasattr(self, "ikine_a"): - ik = self.ikine_a # type: ignore - else: - ik = self.ikine_LM - - q1 = ik(T1, **kwargs) - q2 = ik(T2, **kwargs) + if isinstance(q0, SE3) or isinstance(qf, SE3): + raise TypeError( + "jtraj no longer accepts SE3 poses, this was deprecated in 1.5.0. " + "Solve the inverse kinematics explicitly and pass joint " + "coordinates, e.g. robot.jtraj(robot.ikine_LM(T0).q, " + "robot.ikine_LM(Tf).q, t)" + ) - return rtb.jtraj(q1.q, q2.q, t) + return rtb.jtraj(q0, qf, t, qd0=qd0, qd1=qd1) @overload def jacob0_dot( diff --git a/tests/test_Robot.py b/tests/test_Robot.py index 953f39fe7..8db272ef8 100644 --- a/tests/test_Robot.py +++ b/tests/test_Robot.py @@ -7,7 +7,9 @@ import roboticstoolbox as rtb import unittest import os +import warnings import spatialgeometry as sg +from spatialmath import SE3 from spatialmath.base import tr2jac from tests import skip_no_collision_checking @@ -1058,22 +1060,51 @@ def test_minsingular(self): self.assertAlmostEqual(m, 0.209013, places=4) # type: ignore def test_jtraj(self): + # joint coordinates in, the pure form, no warning r = rtb.models.Panda() - q1 = r.q + 0.2 - q = r.jtraj(r.fkine(q1), r.fkine(r.qr), 5) + with warnings.catch_warnings(): + warnings.simplefilter("error") + traj = r.jtraj(q1, r.qr, 5) - self.assertEqual(q.s.shape, (5, 7)) + self.assertEqual(traj.s.shape, (5, 7)) + self.assertEqual(traj.q.shape, (5, 7)) + nt.assert_allclose(traj.q[0], q1, atol=1e-12) + nt.assert_allclose(traj.q[-1], r.qr, atol=1e-12) - def test_jtraj2(self): - r = rtb.models.DH.Puma560() + # the same as the function + nt.assert_allclose(traj.q, rtb.jtraj(q1, r.qr, 5).q) - q1 = r.q + 0.2 + def test_jtraj_velocities(self): + r = rtb.models.Panda() + qd0 = np.full(7, 0.1) + qd1 = np.full(7, -0.1) + + traj = r.jtraj(r.qz, r.qr, 50, qd0=qd0, qd1=qd1) + + nt.assert_allclose(traj.qd[0], qd0, atol=1e-9) + nt.assert_allclose(traj.qd[-1], qd1, atol=1e-9) + + def test_jtraj_unexpected_keyword(self): + # these used to be forwarded to the inverse kinematics method + r = rtb.models.Panda() + with self.assertRaises(TypeError): + r.jtraj(r.qz, r.qr, 5, foo=1) + + def test_jtraj_rejects_poses(self): + # deprecated in 1.5.0, the message says what to do instead + for r in (rtb.models.Panda(), rtb.models.DH.Puma560()): + T = r.fkine(r.qr) + + with self.assertRaisesRegex(TypeError, "deprecated.*ikine"): + r.jtraj(T, r.qr, 5) - q = r.jtraj(r.fkine(q1), r.fkine(r.qr), 5) + with self.assertRaisesRegex(TypeError, "deprecated.*ikine"): + r.jtraj(r.qr, T, 5) - self.assertEqual(q.s.shape, (5, 6)) + with self.assertRaisesRegex(TypeError, "deprecated.*ikine"): + r.jtraj(T, T, 5) def test_manip(self): r = rtb.models.Panda()