From 02945cdb9415165b89a38d3d9e019094f5ce55de Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 4 Oct 2026 16:55:47 -0400 Subject: [PATCH] feat(robot): jtraj takes joint coordinates only, SE3 arguments raise Robot.jtraj was a quintic trajectory between two joint vectors, with a hack that also accepted SE3 poses and solved the inverse kinematics. IK is never simple (which solution is chosen depends on the solver and its starting point, and it can fail), and the hack never checked for failure: it interpolated silently to an invalid configuration. jtraj(q0, qf, t, qd0=None, qd1=None) is now the method form of rtb.tools.trajectory.jtraj. Passing an SE3 for q0 or qf, deprecated in 1.5.0, raises TypeError with a message saying to solve the IK explicitly. The **kwargs that were forwarded to the IK method are gone, and the arguments, formerly T1 and T2, are now q0 and qf. This is a small incompatible change, released as a feat as is usual for such changes here. It also removes the last caller of Puma560.ikine_a by name. Nothing in the repository, its docs or examples, or RVC3 passed poses. Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/robot/Robot.py | 67 +++++++++++++++++++----------- tests/test_Robot.py | 47 +++++++++++++++++---- 2 files changed, 81 insertions(+), 33 deletions(-) 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()