diff --git a/src/roboticstoolbox/tools/trajectory.py b/src/roboticstoolbox/tools/trajectory.py index 87f3d85a4..95386c8ab 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -724,7 +724,7 @@ def __init__(self, q0: float, qf: float, T: float, V: float | None = None) -> No self._a = 0.0 else: self.tb = (q0 - qf + vel * T) / vel - self._a = vel / self.tb + self._a = 0.0 if self.tb == 0 else vel / self.tb self.V = vel self._q0 = q0 @@ -753,6 +753,10 @@ def __call__(self, t: Any) -> Any: for k, tk in enumerate(times.flat): if tk < 0: pk, pdk, pddk = q0, 0.0, 0.0 + elif tb == 0 and tk <= T: + pk = q0 + V * tk + pdk = V + pddk = 0.0 elif tk <= tb: # initial blend pk = q0 + a / 2 * tk**2 diff --git a/tests/test_trajectory.py b/tests/test_trajectory.py index f067dc563..07c0ea6c5 100644 --- a/tests/test_trajectory.py +++ b/tests/test_trajectory.py @@ -87,6 +87,16 @@ def test_trapezoidal_time_origin(self) -> None: getattr(actual, field), getattr(expected, field), atol=1e-12 ) + def test_trapezoidal_zero_blend_velocity_boundary(self) -> None: + for q0, qf in ((0.0, 1.0), (1.0, 0.0)): + with self.subTest(q0=q0, qf=qf): + trajectory = tr.trapezoidal(q0, qf, np.array([0.0, 0.5, 1.0]), V=1.0) + + self.assertEqual(trajectory.tblend, 0.0) + nt.assert_allclose(trajectory.q, [q0, 0.5, qf]) + nt.assert_allclose(trajectory.qd, np.full(3, qf - q0)) + nt.assert_allclose(trajectory.qdd, np.zeros(3)) + def test_mtraj_time_origin(self) -> None: elapsed = np.linspace(0, 2, 9) for generator in (tr.quintic, tr.trapezoidal):