Skip to content

fix: populate mstraj velocity and acceleration #706

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’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

fix: populate mstraj velocity and acceleration #706

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
77 changes: 46 additions & 31 deletions src/roboticstoolbox/tools/trajectory.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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
Expand Down Expand Up @@ -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.
Expand All @@ -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.
Expand Down Expand Up @@ -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")

Expand Down Expand Up @@ -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

Expand All @@ -1123,20 +1131,27 @@ 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
qd_prev = qd

# 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
Expand Down
44 changes: 44 additions & 0 deletions tests/test_trajectory.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
Loading