Trajectories

A trajectory is how something moves from one place to another: the value of each coordinate at each instant of time. For a robot it is the way to get an arm to make a smooth motion instead of jumping to its goal. A trajectory can be of the joint coordinates, in which case the robot’s joints follow it, or of the pose of the end-effector, which the robot follows by solving inverse kinematics for each point of it.

The Toolbox makes trajectories as arrays, one row for each time step, so that they can be given directly to the methods of a robot. The scalar and joint-space functions return a Trajectory object, which holds the position q, velocity qd and acceleration qdd, and can plot itself. The Cartesian function returns a sequence of poses.

>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> tg = rtb.jtraj(puma.qz, puma.qr, 50)  # 50 steps from one configuration to another
>>> tg.q.shape
(50, 6)
>>> T = puma.fkine(tg.q)  # the pose of the end-effector at each step
>>> len(T)
50

Functions that make trajectories

`jtraj`	Compute a joint-space trajectory
`ctraj`	Cartesian trajectory between two poses
`mtraj`	Multi-axis trajectory
`mstraj`	Multi-segment multi-axis trajectory
`trapezoidal`	Scalar trapezoidal trajectory
`quintic`	Generate scalar polynomial trajectory
`trapezoidal_func`	Trapezoidal scalar profile as a function
`quintic_func`	Quintic scalar polynomial as a function

jtraj is the joint-space trajectory between two configurations, which a robot also has as the method jtraj(), and ctraj is the Cartesian trajectory between two poses. mtraj makes a multi-axis trajectory by applying a scalar function, trapezoidal or quintic, to each axis, and mstraj passes through a sequence of via points. The _func versions return a function of time instead of an array.

Functions and methods that use trajectories

`fkine`	Forward kinematics
`ikine_LM`	Levenberg-Marquardt Numerical Inverse Kinematics Solver
`roboticstoolbox.robot.Robot.Robot.plot`	Graphical display and animation
`rne`	Compute inverse dynamics via recursive Newton-Euler formulation
`roboticstoolbox.tools.trajectory.Trajectory.plot`	Plot trajectory
`xplot`	Plot trajectory data

A joint trajectory can be given to fkine to get the pose of the end-effector at each step, to plot to animate the robot, and, with its velocity and acceleration, to rne to get the joint torques needed to follow it. A pose trajectory is given to the ikine_XX methods, see Inverse Kinematics, to get the joint trajectory that follows it. plot and xplot plot a trajectory against time.

Reference

class Trajectory(name, t, s, sd=None, sdd=None, istime=False)[source]

Bases: object

A container class for trajectory data.

tblend: float: blend duration, set by trapezoidal() only

arrive: ndarray: arrival time at each via point, set by mstraj() only

info: list[Any]: one named tuple per segment (slowest axis, segment time, clock), set by mstraj() only

via: ndarray: the via points, set by mstraj() only

__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) – independent variable, eg. time or step
s (ndarray) – position
sd (ndarray | None) – velocity
sdd (ndarray | None) – acceleration
istime (bool) – t is time, otherwise step number

The object has attributes:

t the independent variable
s the position
sd the velocity
sdd the acceleration

and, set by the function that created it, tblend for trapezoidal() and arrive, info and via for mstraj().

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.

References:

Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3.

__str__()[source]

Return str(self).

Return type:: str

__repr__()[source]

Return repr(self).

Return type:: str

__len__()[source]

Length of trajectory

Returns:: number of steps in the trajectory
Return type:: int

property q: ndarray

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: ndarray

Velocity trajectory

Returns:: trajectory velocity with one row per timestep, one column per axis
Return type:: ndarray(n,m)
Raises:: ValueError – the trajectory has no velocity data

Note

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

property qdd: ndarray

Acceleration trajectory

Returns:: trajectory acceleration with one row per timestep, one column per axis
Return type:: ndarray(n,m)
Raises:: ValueError – the trajectory has no acceleration data

Note

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

property naxes: int

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) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to False
plotargs (dict[str, Any] | None) – Matplotlib options for the plotted lines
textargs (dict[str, Any] | None) – Matplotlib options for the axis labels

Return type:

None

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

quintic and trapezoidal show the individual points with markers
trapezoidal color code the different motion phases
jtraj general m-axis trajectory, show legend

See also

quintic(), trapezoidal()

Changed in version 1.5.0: block=None does not call plt.show, as documented. It used to call plt.show(block=None), which waits in a script. The default, False, is unchanged.

qplot(**kwargs)[source]

Plot multi-axis trajectory

Parameters:: **kwargs (Any) – optional arguments passed to xplot
Return type:: None

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

See also

xplot

__annotations__ = {'arrive': <class 'numpy.ndarray'>, 'info': list[typing.Any], 'tblend': <class 'float'>, 'via': <class 'numpy.ndarray'>}

quintic(q0, qf, t, qd0=0, qdf=0)[source]

Generate scalar polynomial trajectory

Parameters:

q0 (float) – initial value
qf (float) – final value
t (int | ndarray | Sequence[float]) – time
qd0 (float) – initial velocity, optional
qdf (float) – final velocity, optional

Returns:

trajectory

Return type:

Trajectory

tg = quintic(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 = quintic(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.

Example:

>>> from roboticstoolbox import quintic
>>> tg = quintic(1, 2, 10)
>>> tg
Trajectory created by quintic: 10 time steps x 1 axes
>>> len(tg)
10
>>> tg.q
array([1.    , 1.0115, 1.0764, 1.2099, 1.3967, 1.6033, 1.7901, 1.9236,
       1.9885, 2.    ])

(Source code, png, hires.png, pdf)

Note

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

Changed in version 1.5.0: Accurate for long and short durations, see quintic_func(). A time vector, or an integer, giving fewer than two samples raises ValueError; it previously returned the mean of q0 and qf.

References:

Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3.

See also

trapezoidal(), mtraj().

quintic_func(q0, qf, T, qd0=0, qdf=0, method='lstsq')[source]

Quintic scalar polynomial as a function

Parameters:

q0 (float) – initial value
qf (float) – final value
T (float) – trajectory duration, a finite non-zero value
qd0 (float) – initial velocity, defaults to 0
qdf (float) – final velocity, defaults to 0
method (Literal['lstsq', 'hermite']) – how the polynomial is found, "lstsq" (default) solves the linear system, "hermite" uses the closed-form Hermite basis

Return type:

Callable[[ndarray | Sequence[float]], tuple[ndarray, ndarray, ndarray]]

Returns:

polynomial function \(f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))\)

Raises:

ValueError – T is zero or not finite, method is unknown, or an endpoint value is not a scalar

Returns a function which computes the specific quintic polynomial, and its derivatives, as described by the parameters. Velocity and acceleration are with respect to the units of T, and the acceleration is zero at both ends.

The polynomial \(q(t) = a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 t^2 + a_1 t + a_0\) is found from the position, velocity and acceleration at the start and end

\[\begin{split}\begin{pmatrix} 0 & 0 & 0 & 0 & 0 & 1 \\ T^5 & T^4 & T^3 & T^2 & T & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 5T^4 & 4T^3 & 3T^2 & 2T & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 20T^3 & 12T^2 & 6T & 2 & 0 & 0 \end{pmatrix} \begin{pmatrix} a_5 \\ a_4 \\ a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix} = \begin{pmatrix} q_0 \\ q_f \\ \dot{q}_0 \\ \dot{q}_f \\ 0 \\ 0 \end{pmatrix}\end{split}\]

This matrix is badly scaled, since its entries range from 1 to \(T^5\). Its condition number is about 10³ for \(T=1\) but 10¹⁵ for \(T=1000\) and 10¹⁶ for \(T=0.001\), at which point a least-squares solver silently discards a boundary condition: the initial position of quintic(1, 2, 1000) came out near zero rather than one.

For method="lstsq" the system is therefore solved in normalized time \(\tau = t/T\), for which the matrix above is used with \(T=1\). It is the same for every trajectory, with a condition number of 762, and the velocity boundary values are multiplied by \(T\). The derivatives are divided by \(T\) and \(T^2\) to return to the units of t.

For method="hermite" the solution of the same system is written down directly, as a sum of Hermite basis polynomials in \(\tau\). It is equivalent, and is useful as an independent check.

Example:

>>> from roboticstoolbox import quintic_func
>>> f = quintic_func(1, 2, 1000)
>>> f(0)
(np.float64(1.0), np.float64(0.0), np.float64(0.0))
>>> f(1000)
(np.float64(2.0), np.float64(0.0), np.float64(0.0))
>>> f(500)
(np.float64(1.4999999999999996), np.float64(0.0018749999999999982), np.float64(0.0))

Note

The Hermite form is due to Afloat16, who found the loss of accuracy for long and short durations in PR #736.

Changed in version 1.5.0: The system is solved in normalized time. Previously the result was silently wrong for long and short durations, for example the initial value of quintic_func(1, 2, 1000) was close to zero. Added method. A zero or non-finite T raises ValueError, where it previously gave a degenerate result.

See also

quintic() trapezoidal_func()

lspb(*args, **kwargs)[source]

Warning

Deprecated, use trapezoidal instead.

Return type:: Trajectory

trapezoidal(q0, qf, t, V=None)[source]

Scalar trapezoidal trajectory

Parameters:

q0 (float) – initial value
qf (float) – final value
t (int | ndarray | Sequence[float]) – time
V (float | None) – velocity of linear segment, optional

Returns:

trajectory

Return type:

Trajectory

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

tg = trapezoidal(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 = trapezoidal(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.

Example:

>>> from roboticstoolbox import trapezoidal
>>> tg = trapezoidal(1, 2, 10)
>>> tg
Trajectory created by trapezoidal: 10 time steps x 1 axes
>>> len(tg)
10
>>> tg.q
array([1.    , 1.0278, 1.1111, 1.25  , 1.4167, 1.5833, 1.75  , 1.8889,
       1.9722, 2.    ])

(Source code, png, hires.png, pdf)

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. The returned trajectory retains the supplied time vector.
tg has an extra attribute tblend which is the blend duration.

References:

Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3.

class TrapezoidalProfile(q0, qf, T, V=None)[source]

Bases: object

Trapezoidal scalar profile

Parameters:

q0 (float) – initial value
qf (float) – final value
T (float) – duration
V (float | None) – velocity of the linear segment, defaults to 1.5 times the average velocity \((q_f - q_0) / T\)

Raises:

ValueError – V is too small or too big for a solution to exist

An object that is called with a time, or an array of times, and returns the position, velocity and acceleration \((q(t), \dot{q}(t), \ddot{q}(t))\) of a trapezoidal velocity profile: a parabolic blend, a linear segment, and a parabolic blend. A scalar time gives scalar values, an array of times gives arrays of the same shape.

The object has attributes:

tb, the duration of each blend
V, the velocity of the linear segment, signed

Instances are created by trapezoidal_func().

__init__(q0, qf, T, V=None)[source]

tb: float

V: float

__call__(t)[source]

Evaluate the profile

Overloads:

t (float) → tuple[float, float, float]
t (ndarray | Sequence[float]) → tuple[ndarray, ndarray, ndarray]

Parameters:

t (Any) – time, or array of times

Returns:

position, velocity and acceleration, each with the shape of t

__annotations__ = {'V': <class 'float'>, 'tb': <class 'float'>}

trapezoidal_func(q0, qf, T, V=None)[source]

Trapezoidal scalar profile as a function

Parameters:

q0 (float) – initial value
qf (float) – final value
T (float) – maximum time
V (float | None) – velocity of linear segment, defaults to 1.5 times the average

Returns:

trapezoidal profile function \(f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))\)

Return type:

TrapezoidalProfile

Raises:

ValueError – V is too small or too big for a solution to exist

Returns a function which computes the specific trapezoidal profile, and its derivatives, as described by the parameters. The result is called with a time, or an array of times, and has the attributes tb, the blend duration, and V, the velocity of the linear segment.

A scalar time gives scalar values and an array of times gives arrays of the same shape, as for quintic_func().

Changed in version 1.5.0: A scalar time gives scalar values, it previously gave arrays with one element. The result is a TrapezoidalProfile object, which is called in the same way as the function it replaces.

Example:

>>> from roboticstoolbox import trapezoidal_func
>>> f = trapezoidal_func(1, 2, 5)
>>> f(0)
(np.float64(1.0), np.float64(0.0), np.float64(0.17999999999999997))
>>> f(5)
(np.float64(2.0000000000000004), np.float64(0.0), np.float64(-0.17999999999999997))
>>> f(2.5)
(np.float64(1.5), np.float64(0.30000000000000004), np.float64(0.0))
>>> f.tb
1.6666666666666672

mtraj(tfunc, q0, qf, t)[source]

Multi-axis trajectory

Parameters:

tfunc (Callable[..., Trajectory]) – a 1D trajectory function, eg. quintic() or trapezoidal()
q0 (ndarray | Sequence[float]) – initial configuration
qf (ndarray | Sequence[float]) – final configuration
t (int | ndarray | Sequence[float]) – time vector or number of steps

Raises:

TypeError – tfunc is not callable
ValueError – length of q0 and qf are different

Return type:

Trajectory

Returns:

trajectory

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 trapezoidal for a trapezoidal trajectory, or quintic 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.

References:

Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3.

See also

quintic(), trapezoidal()

jtraj(q0, qf, t, qd0=None, qd1=None)[source]

Compute a joint-space trajectory

Parameters:

q0 (ndarray | Sequence[float]) – initial joint coordinate
qf (ndarray | Sequence[float]) – final joint coordinate
t (int | ndarray | Sequence[float]) – time vector or number of steps
qd0 (ndarray | Sequence[float] | None) – initial velocity, defaults to zero
qd1 (ndarray | Sequence[float] | None) – final velocity, defaults to zero

Raises:

ValueError – q0, qf, qd0 and qd1 are not the same size

Return type:

Trajectory

Returns:

trajectory

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.

Note

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

References:

Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3.

See also

ctraj(), xplot, jtraj()

ctraj(T0, T1, t=None, s=None)[source]

Cartesian trajectory between two poses

Parameters:

T0 (SE3) – initial pose
T1 (SE3) – final pose
t (int | ndarray | Sequence[float] | None) – number of samples or time vector
s (ndarray | Sequence[float] | None) – 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

Note

In the second case s could be generated by a scalar trajectory generator such as quintic or trapezoidal (default).
Orientation interpolation is performed using unit-quaternion interpolation.

References:

Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3.

See also

trapezoidal, interp

cmstraj()[source]

Return type:: None

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) – A set of viapoints, one per row
dt (float) – time step
tacc (float | ndarray | Sequence[float]) – acceleration time (seconds)
qdmax (float | ndarray | Sequence[float] | None) – maximum speed, defaults to None
tsegment (ndarray | Sequence[float] | None) – maximum time of each motion segment (seconds), defaults to None
q0 (ndarray | Sequence[float] | None) – initial coordinates, defaults to first row of viapoints
qd0 (ndarray | Sequence[float] | None) – initial velocity, defaults to zero
qdf (ndarray | Sequence[float] | None) – final velocity, defaults to zero
verbose (bool) – print debug information, defaults to False

Returns:

trajectory

Return type:

Trajectory

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:

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)
```
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 returned Trajectory has attributes:

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 (excluding the initial row if q0 is omitted)

Trajectory arrays have one row per time step and one column per axis.

Note

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.
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.
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.

See also

trapezoidal(), ctraj(), mtraj()

xplot(x, y=None, wrist=False, unwrap=False, block=False, labels=None, loc=None, grid=True, stack=False, **kwargs)[source]

Plot trajectory data

Parameters:

x (Any) – the trajectory, one row per timestep, or the time vector if y is given
y (Any) – the trajectory, one row per timestep, 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) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to False
labels (Any) – legend labels, default q0, q1, …
loc (Any) – legend location as per pyplot.legend
grid (bool) – show a grid, defaults to True
stack (bool) – plot each column in its own axes, one above the other, defaults to False
kwargs (Any) – options passed to pyplot.plot

Return type:

Any

Returns:

the axes of the figure

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

xplot(q) plots the joint angles versus row number. If wrist is true and there are six joints, 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.
xplot(t, q) as above but displays the joint angle trajectory versus time given the time vector t, shape (m,).

Example, which is not run when the documentation is built:

import numpy as np
import roboticstoolbox as rtb

puma = rtb.models.DH.Puma560()
tg = rtb.jtraj(puma.qz, puma.qr, 50)  # a trajectory of 50 steps
rtb.xplot(tg.q, wrist=True, labels="q1 q2 q3 q4 q5 q6")

t = np.linspace(0, 2, 50)  # with a time vector
rtb.xplot(t, tg.q)

See also

jtraj(), numpy.unwrap()

Changed in version 1.5.0: block=None does not call plt.show, as documented. It used to call plt.show(block=None), which waits in a script. The default, False, is unchanged.