diff --git a/src/roboticstoolbox/tools/trajectory.py b/src/roboticstoolbox/tools/trajectory.py index 06670cfc1..30d8cffe3 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -868,16 +868,16 @@ def cmstraj(): def mstraj( - viapoints, - dt, - tacc, - qdmax=None, - tsegment=None, - q0=None, - qd0=None, - qdf=None, - verbose=False, -): + viapoints: np.ndarray, + dt: float, + tacc: float | ArrayLike, + qdmax: float | ArrayLike | None = None, + tsegment: ArrayLike | None = None, + q0: ArrayLike | None = None, + qd0: ArrayLike | None = None, + qdf: ArrayLike | None = None, + verbose: bool = False, +) -> Trajectory: """ Multi-segment multi-axis trajectory @@ -894,9 +894,9 @@ def mstraj( :type tsegment: array_like, optional :param q0: initial coordinates, defaults to first row of viapoints :type q0: array_like(n), optional - :param qd0: inital velocity, defaults to zero + :param qd0: initial velocity, defaults to zero :type qd0: array_like(n), optional - :param qdf: final velocity, defaults to zero + :param qdf: final velocity, defaults to zero :type qdf: array_like(n), optional :param verbose: print debug information, defaults to False :type verbose: bool, optional @@ -928,21 +928,23 @@ def mstraj( traj = mstraj(viapoints, dt, tacc, qdmax=SPEED) - The return value is a namedtuple (named ``mstraj``) with elements: + The returned :class:`Trajectory` has attributes: - - ``t`` the time coordinate as a numpy ndarray, shape=(K,) - - ``q`` the axis values as a numpy ndarray, shape=(K,N) - - ``arrive`` a list of arrival times for each segment - - ``info`` a list of named tuples, one per segment that describe the + - ``t`` the time coordinate in seconds, shape=(K,) + - ``q`` the axis values, shape=(K,N) + - ``qd`` the axis velocities in units per second, shape=(K,N) + - ``qdd`` the axis accelerations in units per second squared, shape=(K,N) + - ``arrive`` an array of arrival times for each segment + - ``info`` a list of named tuples, one per segment, that describe the slowest axis, segment time, and time stamp - - ``via`` the passed set of via points + - ``via`` the passed set of via points (excluding the initial row if + ``q0`` is omitted) - The trajectory proper is (``traj.t``, ``traj.q``). The trajectory is a - matrix has one row per time step, and one column per axis. + Trajectory arrays have one row per time step and one column per axis. .. note:: - - Only one of ``qdmag`` or ``tsegment`` can be specified + - Only one of ``qdmax`` or ``tsegment`` can be specified - If ``tacc`` is greater than zero then the path smoothly accelerates between segments using a polynomial blend. This means that the the via point is not actually reached. @@ -954,7 +956,9 @@ def mstraj( correspond to translation and orientation in RPY or Euler angle form. - If ``qdmax`` is a scalar then all axes are assumed to have the same maximum speed. - - ``tg`` has extra attributes ``arrive``, ``info`` and ``via`` + - With ``tacc=0``, velocity changes at via points are instantaneous; + the sampled acceleration is zero within each linear segment. + - The returned trajectory also has ``arrive``, ``info`` and ``via``. :References: - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. @@ -1019,7 +1023,9 @@ def mstraj( clock = 0 # keep track of time arrive = np.zeros((ns,)) # record planned time of arrival at via points - tg = np.zeros((0, nj)) + q_parts = [] + qd_parts = [] + qdd_parts = [] infolist = [] info = namedtuple("mstraj_info", "slowest segtime clock") @@ -1108,12 +1114,14 @@ def mrange(start, stop, step): # add the blend polynomial if taccx > 0: - qb = jtraj( + blend = jtraj( q0, q_prev + tacc2 * qd, mrange(0, taccx, dt), qd0=qd_prev, qd1=qd - ).s + ) if verbose: # pragma nocover - print(qb) - tg = np.vstack([tg, qb[1:, :]]) + print(blend.q) + q_parts.append(blend.q[1:, :]) + qd_parts.append(blend.qd[1:, :]) + qdd_parts.append(blend.qdd[1:, :]) clock = clock + taccx # update the clock @@ -1123,7 +1131,9 @@ def mrange(start, stop, step): q0 = (1 - s) * q_prev + s * q_next # linear step if verbose: # pragma nocover print(t, s, q0) - tg = np.vstack([tg, q0]) + q_parts.append(q0[np.newaxis, :]) + qd_parts.append(qd[np.newaxis, :]) + qdd_parts.append(np.zeros((1, nj))) clock += dt q_prev = q_next # next target becomes previous target @@ -1131,12 +1141,17 @@ def mrange(start, stop, step): # add the final blend if tacc2 > 0: - qb = jtraj(q0, q_next, mrange(0, tacc2, dt), qd0=qd_prev, qd1=qdf).s - tg = np.vstack([tg, qb[1:, :]]) + blend = jtraj(q0, q_next, mrange(0, tacc2, dt), qd0=qd_prev, qd1=qdf) + q_parts.append(blend.q[1:, :]) + qd_parts.append(blend.qd[1:, :]) + qdd_parts.append(blend.qdd[1:, :]) infolist.append(info(None, tseg, clock)) - traj = Trajectory("mstraj", dt * np.arange(0, tg.shape[0]), tg) + q = np.vstack(q_parts) + qd = np.vstack(qd_parts) + qdd = np.vstack(qdd_parts) + traj = Trajectory("mstraj", dt * np.arange(0, q.shape[0]), q, qd, qdd, istime=True) traj.arrive = arrive traj.info = infolist traj.via = viapoints diff --git a/tests/test_trajectory.py b/tests/test_trajectory.py index 45ef98034..f067dc563 100644 --- a/tests/test_trajectory.py +++ b/tests/test_trajectory.py @@ -746,6 +746,50 @@ def test_mstraj(self): with self.assertRaises(ValueError): tr.mstraj(via, dt=1, tacc=1, qdmax=[2, 1], qdf=[1, 2, 3], q0=[1, 2]) + def test_mstraj_derivatives_without_blends(self): + via = np.array([[0.0, 0.0], [1.0, 2.0], [3.0, 0.0]]) + out = tr.mstraj(via, dt=0.1, tacc=0, tsegment=[1.0, 2.0]) + + self.assertEqual(out.q.shape, (30, 2)) + self.assertEqual(out.qd.shape, out.q.shape) + self.assertEqual(out.qdd.shape, out.q.shape) + self.assertTrue(out.istime) + nt.assert_allclose(out.qd[:10], np.tile([1.0, 2.0], (10, 1))) + nt.assert_allclose(out.qd[10:], np.tile([1.0, -1.0], (20, 1))) + nt.assert_array_equal(out.qdd, np.zeros_like(out.q)) + + def test_mstraj_derivatives_with_blends(self): + dt = 0.005 + qdf = [-0.05, 0.1] + out = tr.mstraj( + np.array([[0.0, 0.0], [1.0, -2.0]]), + dt=dt, + tacc=0.4, + qdmax=1.0, + qd0=[0.1, -0.2], + qdf=qdf, + ) + + self.assertEqual(out.qd.shape, out.q.shape) + self.assertEqual(out.qdd.shape, out.q.shape) + self.assertTrue(np.isfinite(out.qd).all()) + self.assertTrue(np.isfinite(out.qdd).all()) + nt.assert_allclose(out.qd[-1], qdf, atol=1e-12) + nt.assert_allclose(out.qdd[-1], [0, 0], atol=1e-10) + + # Sampled velocities and accelerations must describe the returned path, + # including the polynomial blends at either end of the segment. + for i in (5, 10, -11, -6): + with self.subTest(index=i): + nt.assert_allclose( + out.qd[i], (out.q[i + 1] - out.q[i - 1]) / (2 * dt), atol=0.01 + ) + nt.assert_allclose( + out.qdd[i], + (out.q[i + 1] - 2 * out.q[i] + out.q[i - 1]) / dt**2, + atol=0.1, + ) + if __name__ == "__main__": # pragma nocover unittest.main()