Trajectories

class roboticstoolbox.tools.trajectory.Trajectory(name, t, s, sd=None, sdd=None, istime=False)[source]

Bases: object

A container class for trajectory data.

__init__(name, t, s, sd=None, sdd=None, istime=False)[source]

Construct a new trajectory instance

Parameters
  • name (str) – name of the function that created the trajectory

  • t (ndarray(m)) – independent variable, eg. time or step

  • s (ndarray(m) or ndarray(m,n)) – position

  • sd (ndarray(m) or ndarray(m,n)) – velocity

  • sdd (ndarray(m) or ndarray(m,n)) – acceleration

  • istime (float) – t is time, otherwise step number

  • tblend – blend duration (lspb only)

The object has attributes:

  • t the independent variable

  • s the position

  • sd the velocity

  • sdd the acceleration

If t is time, ie. istime is True, then the units of sd and sdd are \(s^{-1}\) and \(s^{-2}\) respectively, otherwise with respect to t.

Note

Data is stored with timesteps as rows and axes as columns.

__str__()[source]

Return str(self).

__repr__()[source]

Return repr(self).

__len__()[source]

Length of trajectory

Returns

number of steps in the trajectory

Return type

int

property q

Position trajectory

Returns

trajectory with one row per timestep, one column per axis

Return type

ndarray(n,m)

Note

This is a synonym for .s, for compatibility with other applications.

property qd

Velocity trajectory

Returns

trajectory velocity with one row per timestep, one column per axis

Return type

ndarray(n,m)

Note

This is a synonym for .sd, for compatibility with other applications.

property qdd

Velocity trajectory

Returns

trajectory acceleration with one row per timestep, one column per axis

Return type

ndarray(n,m)

Note

This is a synonym for .sdd, for compatibility with other applications.

property naxes

Number of axes in the trajectory

Returns

number of axes or dimensions

Return type

int

plot(block=False, plotargs=None, textargs=None)[source]

Plot trajectory

Parameters

block (bool) – wait till plot is dismissed

Plot the position, velocity and acceleration data. The format of the plot depends on the function that created it.

  • tpoly and lspb show the individual points with markers

  • lspb color code the different motion phases

  • jtraj general m-axis trajectory, show legend

Seealso

tpoly(), lspb()

qplot(**kwargs)[source]

Plot multi-axis trajectory

Parameters

**kwargs

optional arguments passed to qplot

Plots a multi-axis trajectory, held within the object, as position against time.

Seealso

qplot()

__dict__ = mappingproxy({'__module__': 'roboticstoolbox.tools.trajectory', '__doc__': '\n    A container class for trajectory data.\n    ', '__init__': <function Trajectory.__init__>, '__str__': <function Trajectory.__str__>, '__repr__': <function Trajectory.__repr__>, '__len__': <function Trajectory.__len__>, 'q': <property object>, 'qd': <property object>, 'qdd': <property object>, 'naxes': <property object>, 'plot': <function Trajectory.plot>, 'qplot': <function Trajectory.qplot>, '__dict__': <attribute '__dict__' of 'Trajectory' objects>, '__weakref__': <attribute '__weakref__' of 'Trajectory' objects>, '__annotations__': {}})
__module__ = 'roboticstoolbox.tools.trajectory'
__weakref__

list of weak references to the object (if defined)

roboticstoolbox.tools.trajectory.tpoly(q0, qf, t, qd0=0, qdf=0)[source]

Generate scalar polynomial trajectory

Parameters
  • q0 (float) – initial value

  • qf (float) – final value

  • t (int or array_like(m)) – time

  • qd0 – initial velocity, optional

  • qdf – final velocity, optional

Returns

trajectory

Return type

Trajectory instance

  • tg = tpoly(q0, q1, m) is a scalar trajectory (Mx1) that varies smoothly from q0 to qf using a quintic polynomial. The initial and final velocity and acceleration are zero. m is an integer scalar, indicating the total number of timesteps and

    • Velocity is in units of distance per trajectory step, not per second.

    • Acceleration is in units of distance per trajectory step squared, not per second squared.

  • tg = tpoly(q0, q1, t) as above but t is a uniformly-spaced time vector

    • Velocity is in units of distance per second.

    • Acceleration is in units of distance per second squared.

The return value is an object that contains position, velocity and acceleration data.

Note

The time vector T is assumed to be monotonically increasing, and time scaling is based on the first and last element.

References:

  • Robotics, Vision & Control, Chap 3, P. Corke, Springer 2011.

Seealso

lspb(), mtraj().

roboticstoolbox.tools.trajectory.tpoly_func(q0, qf, T, qd0=0, qdf=0)[source]
roboticstoolbox.tools.trajectory.lspb(q0, qf, t, V=None)[source]

Scalar trapezoidal trajectory

Parameters
  • q0 (float) – initial value

  • qf (float) – final value

  • t (float or array_like) – time

  • V (float) – velocity of linear segment, optional

Returns

trajectory

Return type

Trajectory instance

Computes a trapezoidal trajectory, which has a linear motion segment with parabolic blends.

  • tg = lspb(q0, qf, t) is a scalar trajectory (Mx1) that varies smoothly from q0 to qf in M steps using a constant velocity segment and parabolic blends. Time t can be either:

    • an integer scalar, indicating the total number of timesteps

      • Velocity is in units of distance per trajectory step, not per second.

      • Acceleration is in units of distance per trajectory step squared, not per second squared.

    • an array_like, containing the time steps.

      • Results are scaled to units of time.

  • tg = lspb(q0, q1, t, V) as above but specifies the velocity of the linear segment which is normally computed automatically.

The return value is an object that contains position, velocity and acceleration data.

Note

  • For some values of V no solution is possible and an error is flagged.

  • The time vector, if given, is assumed to be monotonically increasing, and time scaling is based on the first and last element.

  • tg has an extra attribute xblend which is the blend duration.

References
  • Robotics, Vision & Control, Chap 3,

  1. Corke, Springer 2011.

Seealso

tpoly(), mtraj().

roboticstoolbox.tools.trajectory.lspb_func(q0, qf, tf, V=None)[source]
roboticstoolbox.tools.trajectory.jtraj(q0, qf, t, qd0=None, qd1=None)[source]

Compute a joint-space trajectory

Parameters
  • q0 (array_like(n)) – initial joint coordinate

  • qf (array_like(n)) – final joint coordinate

  • t (array_like or int) – time vector or number of steps

  • qd0 (array_like(n), optional) – initial velocity, defaults to zero

  • qd1 (array_like(n), optional) – final velocity, defaults to zero

Returns

trajectory

Return type

Trajectory instance

  • tg = jtraj(q0, qf, N) is a joint space trajectory where the joint coordinates vary from q0 (M) to qf (M). A quintic (5th order) polynomial is used with default zero boundary conditions for velocity and acceleration. Time is assumed to vary from 0 to 1 in N steps.

  • tg = jtraj(q0, qf, t) as above but t is a uniformly-spaced time vector

The return value is an object that contains position, velocity and acceleration data.

Notes:

  • The time vector, if given, scales the velocity and acceleration outputs assuming that the time vector starts at zero and increases linearly.

Seealso

ctraj(), qplot(), jtraj()

roboticstoolbox.tools.trajectory.mtraj(tfunc, q0, qf, t)[source]

Multi-axis trajectory

Parameters
  • tfunc (callable) – a 1D trajectory function, eg. tpoly or lspb

  • q0 (ndarray(m)) – initial configuration

  • qf (ndarray(m)) – final configuration

  • t (array_like or int) – time vector or number of steps

Raises
  • TypeErrortfunc is not callable

  • ValueError – length of q0 and qf are different

Returns

trajectory

Return type

Trajectory instance

  • tg = mtraj(func, q0, qf, n) is a multi-axis trajectory varying from configuration q0 (M) to qf (M) according to the scalar trajectory function tfunc in n` steps.

  • tg = mtraj(func, q0, qf, t) as above but t is a uniformly-spaced time vector

The scalar trajectory function is applied to each axis:

tg = tfunc(s0, sF, n)

and possible values of TFUNC include lspb for a trapezoidal trajectory, or tpoly for a polynomial trajectory.

The return value is an object that contains position, velocity and acceleration data.

Note

The time vector, if given, is assumed to be monotonically increasing, and time scaling is based on the first and last element.

Seealso

tpoly(), lspb()

roboticstoolbox.tools.trajectory.qplot(x, y=None, wrist=False, unwrap=False, block=False, labels=None, loc=None, grid=True, stack=False, **kwargs)[source]

Plot trajectory data

Parameters
  • q (ndarray(m,n)) – trajectory, one row per timestep

  • t (numpy ndarray, shape=(M,)) – time vector, optional

  • wrist (bool) – distinguish arm and wrist joints with line styles

  • unwrap (bool) – unwrap joint angles so that they smoothly increase or decrease when they pass through \(\pm \pi\)

  • block (bool) – block until the plot is closed

  • labels (list of str, or single string with space separated labels) – legend labels

  • kwargs – options passed to pyplot.plot

  • loc (str) – legend location as per pyplot.legend

This is a convenience function to plot trajectories, where each row represents one time step.

  • qplot(q) plots the joint angles versus row number. If N==6 a conventional 6-axis robot is assumed, and the first three joints are shown as solid lines, the last three joints (wrist) are shown as dashed lines. A legend is also displayed.

  • qplot(t, q) as above but displays the joint angle trajectory versus time given the time vector T (Mx1).

Example:

>>> qplot(q, x, labels='x y z')
Seealso

jtraj(), numpy.unwrap()

roboticstoolbox.tools.trajectory.ctraj(T0, T1, t=None, s=None)[source]

Cartesian trajectory between two poses

Parameters
  • T0 (SE3) – initial pose

  • T1 (SE3) – final pose

  • t (int or ndarray(n)) – number of samples or time vector

  • s (ndarray(s)) – array of distance along the path, in the interval [0, 1]

Return T0

smooth path from T0 to T1

Return type

SE3

ctraj(T0, T1, n) is a Cartesian trajectory from SE3 pose T0 to T1 with n points that follow a trapezoidal velocity profile along the path. The Cartesian trajectory is an SE3 instance containing n values.

ctraj(T0, T1, t) as above but the trajectory is sampled at the points in the array t.

ctraj(T0, T1, s=s) as above but the elements of s specify the fractional distance along the path, and these values are in the range [0 1]. The i’th point corresponds to a distance s[i] along the path.

Examples:

>>> tg = ctraj(SE3.Rand(), SE3.Rand(), 20)
>>> len(tg)
20

Notes:

  • In the second case s could be generated by a scalar trajectory generator such as tpoly or lspb (default).

  • Orientation interpolation is performed using unit-quaternion interpolation.

Reference:

  • Robotics, Vision & Control, Sec 3.1.5, Peter Corke, Springer 2011

Seealso

lspb(), interp()

roboticstoolbox.tools.trajectory.cmstraj()[source]
roboticstoolbox.tools.trajectory.mstraj(viapoints, dt, tacc, qdmax=None, tsegment=None, q0=None, qd0=None, qdf=None, verbose=False)[source]

Multi-segment multi-axis trajectory

Parameters
  • viapoints (ndarray(m,n)) – A set of viapoints, one per row

  • dt (float (seconds)) – time step

  • tacc (float) – acceleration time (seconds)

  • qdmax (array_like(n) or float, optional) – maximum speed, defaults to None

  • tsegment (array_like, optional) – maximum time of each motion segment (seconds), defaults to None

  • q0 (array_like(n), optional) – initial coordinates, defaults to first row of viapoints

  • qd0 (array_like(n), optional) – inital velocity, defaults to zero

  • qdf (array_like(n), optional) – final velocity, defaults to zero

  • verbose (bool, optional) – print debug information, defaults to False

Returns

trajectory

Return type

Trajectory instance

Computes a trajectory for N axes moving smoothly through a set of viapoints. The motion comprises M segments:

  • The initial coordinates are the first row of viapoints or q0 if provided.

  • The final coordinates are the last row of viapoints

  • Each segment is linear motion and polynomial blends connect the viapoints.

  • All joints arrive at each via point at the same time, ie. the motion is coordinated across axes

The time of the segments can be specified in two different ways:

  1. In terms of segment time where tsegment is an array of segment times which is the number of via points minus one:

    ``traj = mstraj(viapoints, dt, tacc, tsegment=TS)``
    
  2. Governed by the speed of the slowest axis for the segment. The axis speed is a scalar (all axes have the same speed) or an N-vector of speed per axis:

    traj = mstraj(viapoints, dt, tacc, qdmax=SPEED)
    

The return value is a namedtuple (named mstraj) with elements:

  • 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 slowest axis, segment time, and time stamp

  • via the passed set of via points

The trajectory proper is (traj.t, traj.q). The trajectory is a matrix has one row per time step, and one column per axis.

Notes:

  • Only one of qdmag 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.

  • The path length K is a function of the number of via points and the time or velocity limits that apply.

  • Can be used to create joint space trajectories where each axis is a joint coordinate.

  • Can be used to create Cartesian trajectories where the “axes” 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

Seealso

lspb, ctraj, mtraj