Mobile robot kinematic models

These vehicle kinematic classes have methods to:

  • predict new configuration based on odometry

  • compute configuration derivative

  • simulate and animate motion

  • compute Jacobians

Bicycle model

class roboticstoolbox.mobile.Bicycle(L=1, steer_max=1.413716694115407, **kwargs)[source]

Bases: VehicleBase

__init__(L=1, steer_max=1.413716694115407, **kwargs)[source]

Create bicycle kinematic model

Parameters
  • L (float, optional) – wheel base, defaults to 1

  • steer_max (float, optional) – [description], defaults to \(0.45\pi\)

  • kwargs – additional arguments passed to VehicleBase constructor

Model the motion of a bicycle model with equations of motion given by:

\[\begin{split}\dot{x} &= v \cos \theta \\ \dot{y} &= v \sin \theta \\ \dot{\theta} &= \frac{v}{L} \tan \gamma\end{split}\]

where \(v\) is the velocity in body frame x-direction, and \(\gamma\) is the angle of the steered wheel.

Seealso

f() deriv() Fx() meth:Fv VehicleBase

property l

Vehicle wheelbase

Returns

vehicle wheelbase

Return type

float

property radius_min

Vehicle turning radius

Returns

radius of minimum possible turning circle

Return type

float

Minimum turning radius based on maximum steered wheel angle and wheelbase.

\[\frac{l}{\tan \gamma_{max}}\]
Seealso

curvature_max()

property curvature_max

Vehicle maximum path curvature

Returns

maximum curvature

Return type

float

Maximum path curvature based on maximum steered wheel angle and wheelbase.

\[\frac{\tan \gamma_{max}}{l}\]
Seealso

radius_min()

property steer_max

Vehicle maximum steered wheel angle

Returns

maximum angle

Return type

float

deriv(x, u, limits=True)[source]

Time derivative of state

Parameters
  • x (array_like(3)) – vehicle state \((x, y, \theta)\)

  • u (array_like(2)) – control input \((v, \gamma)\)

Returns

state derivative \((\dot{x}, \dot{y}, \dot{\theta})\)

Return type

ndarray(3)

Returns the time derivative of state (3x1) at the state x with velocity \(v\) and steered wheel angle \(\gamma\)

\[\begin{split}\dot{x} &= v \cos \theta \\ \dot{y} &= v \sin \theta \\ \dot{\theta} &= \frac{v}{L} \tan \gamma\end{split}\]
Seealso

f()

u_limited(u)[source]

Apply vehicle velocity, acceleration and steering limits

Parameters

u (array_like(2)) – Desired vehicle inputs \((v, \gamma)\)

Returns

Allowable vehicle inputs \((v, \gamma)\)

Return type

ndarray(2)

Velocity and acceleration limits are applied to \(v\) and steered wheel angle limits are applied to \(\gamma\).

Fv(x, odo)

Jacobian of state transition function df/dv

Parameters
  • x (array_like(3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

Returns

Jacobian matrix

Return type

ndarray(3,2)

Returns the Jacobian matrix \(\frac{\partial \vec{f}}{\partial \vec{v}}\) for the given state and odometry.

Seealso

f() deriv() Fx()

Fx(x, odo)

Jacobian of state transition function df/dx

Parameters
  • x (array_like(3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

Returns

Jacobian matrix

Return type

ndarray(3,3)

Returns the Jacobian matrix \(\frac{\partial \vec{f}}{\partial \vec{x}}\) for the given state and odometry.

Seealso

f() deriv() Fv()

property accel_max

Get maximum acceleration of vehicle (superclass)

Returns

maximum acceleration

Return type

float

Set by VehicleBase() subclass constructor.

Seealso

speed_max() steer_max() u_limited()

add_driver(driver)

Add a driver agent (superclass)

Parameters

driver (VehicleDriverBase subclass) – a driver agent object

Warning

Deprecated. Use vehicle.control = driver instead.

Seealso

VehicleDriverBase control()

property control

Get/set vehicle control (superclass)

Getter

Returns vehicle’s control

Setter

Sets the vehicle’s control

Type

2-tuple, callable, interp1d or VehicleDriver

This is the input to the vehicle during a simulation performed by run(). The control input can be:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple

  • a driver agent, subclass of driversVehicleDriverBase

Example:

>>> from roboticstoolbox import Bicycle, RandomPath
>>> bike = Bicycle()
>>> bike.control = RandomPath(10)
>>> print(bike)
Bicycle: x = [ 0, 0, 0 ]
  L=1, steer_max=1.41372, speed_max=inf, accel_max=inf
Seealso

run() eval_control() scipy.interpolate.interp1d VehicleDriverBase

property dt

Get sample time (superclass)

Returns

discrete time step for simulation

Return type

float

Set by VehicleBase() subclass constructor. Used by step() to update state.

Seealso

run()

eval_control(control, x)

Evaluate vehicle control input (superclass method)

Parameters
  • control ([type]) – vehicle control

  • x (array_like(3)) – vehicle state \((x, y, heta)\)

Raises

ValueError – bad control

Returns

vehicle control inputs

Return type

ndarray(2)

Evaluates the control for this time step and state. Control is set by the control() property to:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple

  • a driver agent, subclass of VehicleDriverBase

Vehicle steering, speed and acceleration limits are applied to the result before being input to the vehicle model.

Seealso

run() control() scipy.interpolate.interp1d()

f(x, odo, v=None)

State transition function

Parameters
  • x (array_like(3), ndarray(n,3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

  • v (array_like(2), optional) – additive odometry noise, defaults to (0,0)

Returns

predicted vehicle state

Return type

ndarray(3)

Predict the next state based on current state and odometry value. v is a random variable that represents additive odometry noise for simulation purposes.

\[f: \vec{x}_k, \delta_d, \delta_\theta \mapsto \vec{x}_{k+1}\]

For particle filters it is useful to apply the odometry to the states of all particles and this can be achieved if x is a 2D array with one state per row. v is ignored in this case.

Note

This is the state update equation used for EKF localization.

Seealso

deriv() Fx() Fv()

init(x0=None, control=None)

Initialize for simulation (superclass)

Parameters

x0 (array_like(3) or array_like(2)) – Initial state, defaults to value given to Vehicle constructor

Performs the following initializations:

  1. Clears the state history

  2. Set the private random number generator to initial value

  3. Sets state \(x = x_0\)

  4. Sets previous velocity to zero

  5. If a driver is attached, initialize it

  6. If animation is enabled, initialize that

If animation is enabled by constructor and no animation object is given, use a default VehiclePolygon("car")

Seealso

VehicleAnimationBase run()

limits_va(v)

Apply velocity and acceleration limits (superclass)

Parameters

v (float) – desired velocity

Returns

allowed velocity

Return type

float

Determine allowable velocity given desired velocity, speed and acceleration limits.

Note

This function is stateful, requires previous velocity, _v_prev attribute, to enable acceleration limiting. This is reset to zero at the start of each simulation.

plot_xy(*args, block=False, **kwargs)

Plot xy-path from history

Parameters
  • block (bool, optional) – block until plot dismissed, defaults to False

  • args – positional arguments passed to plot()

  • kwargs – keyword arguments passed to plot()

The \((x,y)\) trajectory from the simulation history is plotted as \(x\) vs :math:`y.

Seealso

run() plot_xyt()

plot_xyt(block=False, **kwargs)

Plot configuration vs time from history

Parameters
  • block (bool, optional) – block until plot dismissed, defaults to False

  • args – positional arguments passed to plot()

  • kwargs – keyword arguments passed to plot()

The \((x,y, heta)\) trajectory from the simulation history is plotted against time.

Seealso

run() plot_xy()

polygon(q)

Bounding polygon at vehicle configuration

Parameters

q (array_like(3)) – vehicle configuration \((x, y, heta)\)

Returns

bounding polygon of vehicle at configuration q

Return type

Polygon2

The bounding polygon of the vehicle is returned for the configuration q. Can be used for collision checking using the intersects() method.

Seealso

Polygon2

property q

Get vehicle state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Seealso

x()

property random

Get private random number generator

Returns

NumPy random number generator

Return type

numpy.random.Generator

Has methods including:

The generator is initialized with the seed provided at constructor time every time init() is called.

Seealso

init() numpy.random.default_rng()

run(T=10, x0=None, control=None, animate=False)

Simulate motion of vehicle (superclass)

Parameters
  • N (int, optional) – Number of simulation steps, defaults to 1000

  • x0 (array_like(3) or array_like(2)) – Initial state, defaults to value given to Vehicle constructor

  • animation (VehicleAnimation subclass, optional) – vehicle animation object, defaults to None

  • plot (bool, optional) – Enable plotting, defaults to False

Returns

State trajectory, each row is \((x,y,\theta)\).

Return type

ndarray(n,3)

Runs the vehicle simulation for N timesteps and optionally plots an animation. The method step() performs each time step.

The control inputs are providd by control which can be:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple, see SciPy interp1d

  • a driver agent, subclass of VehicleDriverBase

This is evaluated by eval_control() which applies velocity, acceleration and steering limits.

The simulation can be stopped prematurely by the control function calling stopif().

Seealso

init() step() control()

property speed_max

Get maximum speed of vehicle (superclass)

Returns

maximum speed

Return type

float

Set by VehicleBase() subclass constructor.

Seealso

accel_max() steer_max() u_limited()

step(u=None, animate=False)

Step simulator by one time step (superclass)

Returns

odometry \((\delta_d, \delta_\theta)\)

Return type

ndarray(2)

  1. Obtain the vehicle control inputs

  2. Integrate the vehicle state forward one timestep

  3. Updates the stored state and state history

  4. Update animation if enabled.

  5. Returns the odometry

  • veh.step((vel, steer)) for a Bicycle vehicle model

  • veh.step((vel, vel_diff)) for a Unicycle vehicle model

  • veh.step() as above but control is taken from the control attribute which might be a function or driver agent.

Example:

>>> from roboticstoolbox import Bicycle
>>> bike = Bicycle()  # default bicycle model
>>> bike.step((1, 0.2))  # one step: v=1, γ=0.2
array([0.1   , 0.0203])
>>> bike.x
array([0.1   , 0.    , 0.0203])
>>> bike.step((1, 0.2))  # another step: v=1, γ=0.2
array([0.1   , 0.0203])
>>> bike.x
array([0.2   , 0.002 , 0.0405])

Note

Vehicle control input limits are applied.

Seealso

control(), update(), run()

stopsim()

Stop the simulation (superclass)

A control function can stop the simulation initated by the run method.

Seealso

run()

property verbose

Get verbosity (superclass)

Returns

verbosity level

Return type

bool

Set by VehicleBase subclass constructor.

property workspace

Size of robot workspace (superclass)

Returns

workspace bounds [xmin, xmax, ymin, ymax]

Return type

ndarray(4)

Returns the bounds of the workspace as specified by constructor option workspace

property x

Get vehicle state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Seealso

q()

property x0

Get vehicle initial state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Set by VehicleBase subclass constructor. The state is set to this value at the beginning of each simulation run.

Seealso

run()

property x_hist

Get vehicle state/configuration history (superclass)

Returns

Vehicle state history

Return type

ndarray(n,3)

The state \((x, y, heta)\) at each time step resulting from a simulation run, one row per timestep.

Seealso

run()

Unicycle model

class roboticstoolbox.mobile.Unicycle(W=1, steer_max=inf, **kwargs)[source]

Bases: VehicleBase

__init__(W=1, steer_max=inf, **kwargs)[source]

Create unicycle kinematic model

Parameters
  • W (float, optional) – vehicle width, defaults to 1

  • steer_max (float) – maximum turn rate

  • kwargs – additional arguments passed to VehicleBase constructor

Model the motion of a unicycle model with equations of motion given by:

\[\begin{split}\dot{x} &= v \cos \theta \\ \dot{y} &= v \sin \theta \\ \dot{\theta} &= \omega\end{split}\]

where \(v\) is the velocity in body frame x-direction, and \(\omega\) is the turn rate.

Seealso

f() deriv() Fx() meth:Fv Vehicle

deriv(t, x, u)[source]

Time derivative of state

Parameters
  • x (array_like(3)) – vehicle state \((x, y, \theta)\)

  • u (array_like(2)) – control input \((v, \omega)\)

Returns

state derivative \((\dot{x}, \dot{y}, \dot{\theta})\)

Return type

ndarray(3)

Returns the time derivative of state (3x1) at the state x with velocity \(v\) and turn rate \(\omega\)

\[\begin{split}\dot{x} &= v \cos \theta \\ \dot{y} &= v \sin \theta \\ \dot{\theta} &= \omega\end{split}\]
Seealso

f()

Note

Vehicle speed and steering limits are not applied here

u_limited(u)[source]

Apply vehicle velocity, acceleration and steering limits

Parameters

u (array_like(2)) – Desired vehicle inputs \((v, \omega)\)

Returns

Allowable vehicle inputs \((v, \omega)\)

Return type

ndarray(2)

Velocity and acceleration limits are applied to \(v\) and turn rate limits are applied to \(\omega\).

Fv(x, odo)

Jacobian of state transition function df/dv

Parameters
  • x (array_like(3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

Returns

Jacobian matrix

Return type

ndarray(3,2)

Returns the Jacobian matrix \(\frac{\partial \vec{f}}{\partial \vec{v}}\) for the given state and odometry.

Seealso

f() deriv() Fx()

Fx(x, odo)

Jacobian of state transition function df/dx

Parameters
  • x (array_like(3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

Returns

Jacobian matrix

Return type

ndarray(3,3)

Returns the Jacobian matrix \(\frac{\partial \vec{f}}{\partial \vec{x}}\) for the given state and odometry.

Seealso

f() deriv() Fv()

property accel_max

Get maximum acceleration of vehicle (superclass)

Returns

maximum acceleration

Return type

float

Set by VehicleBase() subclass constructor.

Seealso

speed_max() steer_max() u_limited()

add_driver(driver)

Add a driver agent (superclass)

Parameters

driver (VehicleDriverBase subclass) – a driver agent object

Warning

Deprecated. Use vehicle.control = driver instead.

Seealso

VehicleDriverBase control()

property control

Get/set vehicle control (superclass)

Getter

Returns vehicle’s control

Setter

Sets the vehicle’s control

Type

2-tuple, callable, interp1d or VehicleDriver

This is the input to the vehicle during a simulation performed by run(). The control input can be:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple

  • a driver agent, subclass of driversVehicleDriverBase

Example:

>>> from roboticstoolbox import Bicycle, RandomPath
>>> bike = Bicycle()
>>> bike.control = RandomPath(10)
>>> print(bike)
Bicycle: x = [ 0, 0, 0 ]
  L=1, steer_max=1.41372, speed_max=inf, accel_max=inf
Seealso

run() eval_control() scipy.interpolate.interp1d VehicleDriverBase

property dt

Get sample time (superclass)

Returns

discrete time step for simulation

Return type

float

Set by VehicleBase() subclass constructor. Used by step() to update state.

Seealso

run()

eval_control(control, x)

Evaluate vehicle control input (superclass method)

Parameters
  • control ([type]) – vehicle control

  • x (array_like(3)) – vehicle state \((x, y, heta)\)

Raises

ValueError – bad control

Returns

vehicle control inputs

Return type

ndarray(2)

Evaluates the control for this time step and state. Control is set by the control() property to:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple

  • a driver agent, subclass of VehicleDriverBase

Vehicle steering, speed and acceleration limits are applied to the result before being input to the vehicle model.

Seealso

run() control() scipy.interpolate.interp1d()

f(x, odo, v=None)

State transition function

Parameters
  • x (array_like(3), ndarray(n,3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

  • v (array_like(2), optional) – additive odometry noise, defaults to (0,0)

Returns

predicted vehicle state

Return type

ndarray(3)

Predict the next state based on current state and odometry value. v is a random variable that represents additive odometry noise for simulation purposes.

\[f: \vec{x}_k, \delta_d, \delta_\theta \mapsto \vec{x}_{k+1}\]

For particle filters it is useful to apply the odometry to the states of all particles and this can be achieved if x is a 2D array with one state per row. v is ignored in this case.

Note

This is the state update equation used for EKF localization.

Seealso

deriv() Fx() Fv()

init(x0=None, control=None)

Initialize for simulation (superclass)

Parameters

x0 (array_like(3) or array_like(2)) – Initial state, defaults to value given to Vehicle constructor

Performs the following initializations:

  1. Clears the state history

  2. Set the private random number generator to initial value

  3. Sets state \(x = x_0\)

  4. Sets previous velocity to zero

  5. If a driver is attached, initialize it

  6. If animation is enabled, initialize that

If animation is enabled by constructor and no animation object is given, use a default VehiclePolygon("car")

Seealso

VehicleAnimationBase run()

limits_va(v)

Apply velocity and acceleration limits (superclass)

Parameters

v (float) – desired velocity

Returns

allowed velocity

Return type

float

Determine allowable velocity given desired velocity, speed and acceleration limits.

Note

This function is stateful, requires previous velocity, _v_prev attribute, to enable acceleration limiting. This is reset to zero at the start of each simulation.

plot_xy(*args, block=False, **kwargs)

Plot xy-path from history

Parameters
  • block (bool, optional) – block until plot dismissed, defaults to False

  • args – positional arguments passed to plot()

  • kwargs – keyword arguments passed to plot()

The \((x,y)\) trajectory from the simulation history is plotted as \(x\) vs :math:`y.

Seealso

run() plot_xyt()

plot_xyt(block=False, **kwargs)

Plot configuration vs time from history

Parameters
  • block (bool, optional) – block until plot dismissed, defaults to False

  • args – positional arguments passed to plot()

  • kwargs – keyword arguments passed to plot()

The \((x,y, heta)\) trajectory from the simulation history is plotted against time.

Seealso

run() plot_xy()

polygon(q)

Bounding polygon at vehicle configuration

Parameters

q (array_like(3)) – vehicle configuration \((x, y, heta)\)

Returns

bounding polygon of vehicle at configuration q

Return type

Polygon2

The bounding polygon of the vehicle is returned for the configuration q. Can be used for collision checking using the intersects() method.

Seealso

Polygon2

property q

Get vehicle state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Seealso

x()

property random

Get private random number generator

Returns

NumPy random number generator

Return type

numpy.random.Generator

Has methods including:

The generator is initialized with the seed provided at constructor time every time init() is called.

Seealso

init() numpy.random.default_rng()

run(T=10, x0=None, control=None, animate=False)

Simulate motion of vehicle (superclass)

Parameters
  • N (int, optional) – Number of simulation steps, defaults to 1000

  • x0 (array_like(3) or array_like(2)) – Initial state, defaults to value given to Vehicle constructor

  • animation (VehicleAnimation subclass, optional) – vehicle animation object, defaults to None

  • plot (bool, optional) – Enable plotting, defaults to False

Returns

State trajectory, each row is \((x,y,\theta)\).

Return type

ndarray(n,3)

Runs the vehicle simulation for N timesteps and optionally plots an animation. The method step() performs each time step.

The control inputs are providd by control which can be:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple, see SciPy interp1d

  • a driver agent, subclass of VehicleDriverBase

This is evaluated by eval_control() which applies velocity, acceleration and steering limits.

The simulation can be stopped prematurely by the control function calling stopif().

Seealso

init() step() control()

property speed_max

Get maximum speed of vehicle (superclass)

Returns

maximum speed

Return type

float

Set by VehicleBase() subclass constructor.

Seealso

accel_max() steer_max() u_limited()

step(u=None, animate=False)

Step simulator by one time step (superclass)

Returns

odometry \((\delta_d, \delta_\theta)\)

Return type

ndarray(2)

  1. Obtain the vehicle control inputs

  2. Integrate the vehicle state forward one timestep

  3. Updates the stored state and state history

  4. Update animation if enabled.

  5. Returns the odometry

  • veh.step((vel, steer)) for a Bicycle vehicle model

  • veh.step((vel, vel_diff)) for a Unicycle vehicle model

  • veh.step() as above but control is taken from the control attribute which might be a function or driver agent.

Example:

>>> from roboticstoolbox import Bicycle
>>> bike = Bicycle()  # default bicycle model
>>> bike.step((1, 0.2))  # one step: v=1, γ=0.2
array([0.1   , 0.0203])
>>> bike.x
array([0.1   , 0.    , 0.0203])
>>> bike.step((1, 0.2))  # another step: v=1, γ=0.2
array([0.1   , 0.0203])
>>> bike.x
array([0.2   , 0.002 , 0.0405])

Note

Vehicle control input limits are applied.

Seealso

control(), update(), run()

stopsim()

Stop the simulation (superclass)

A control function can stop the simulation initated by the run method.

Seealso

run()

property verbose

Get verbosity (superclass)

Returns

verbosity level

Return type

bool

Set by VehicleBase subclass constructor.

property workspace

Size of robot workspace (superclass)

Returns

workspace bounds [xmin, xmax, ymin, ymax]

Return type

ndarray(4)

Returns the bounds of the workspace as specified by constructor option workspace

property x

Get vehicle state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Seealso

q()

property x0

Get vehicle initial state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Set by VehicleBase subclass constructor. The state is set to this value at the beginning of each simulation run.

Seealso

run()

property x_hist

Get vehicle state/configuration history (superclass)

Returns

Vehicle state history

Return type

ndarray(n,3)

The state \((x, y, heta)\) at each time step resulting from a simulation run, one row per timestep.

Seealso

run()

Superclass

class roboticstoolbox.mobile.VehicleBase(covar=None, speed_max=inf, accel_max=inf, x0=[0, 0, 0], dt=0.1, control=None, seed=0, animation=None, verbose=False, plot=False, workspace=None, polygon=None)[source]

Bases: ABC

__init__(covar=None, speed_max=inf, accel_max=inf, x0=[0, 0, 0], dt=0.1, control=None, seed=0, animation=None, verbose=False, plot=False, workspace=None, polygon=None)[source]

Superclass for vehicle kinematic models

Parameters
  • covar (ndarray(2,2), optional) – odometry covariance, defaults to zero

  • speed_max (float, optional) – maximum speed, defaults to \(\infty\)

  • accel_max (float, optional) – maximum acceleration, defaults to \(\infty\)

  • x0 (array_like(3), optional) – Initial state, defaults to (0,0,0)

  • dt (float, optional) – sample update interval, defaults to 0.1

  • control (array_like(2), interp1d, VehicleDriverBase) – vehicle control inputs, defaults to None

  • animation (VehicleAnimationBase subclass, optional) – Graphical animation of vehicle, defaults to None

  • verbose (bool, optional) – print lots of info, defaults to False

  • workspace (float, array_like(2), array_like(4)) – dimensions of 2D plot area, defaults to (-10:10) x (-10:10), see plotvol2()

  • polygon (Polygon2) – bounding polygon of vehicle

This is an abstract superclass that simulates the motion of a mobile robot under the action of a controller. The controller provides control inputs to the vehicle, and the output odometry is returned. The true state, effectively unknowable in practice, is computed and accessible.

The workspace can be specified in several ways:

workspace

x-range

y-range

A (scalar)

-A:A

-A:A

[A, B]

A:B

A:B

[A, B, C, D]

A:B

C:D

Seealso

Bicycle Unicycle VehicleAnimationBase

f(x, odo, v=None)[source]

State transition function

Parameters
  • x (array_like(3), ndarray(n,3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

  • v (array_like(2), optional) – additive odometry noise, defaults to (0,0)

Returns

predicted vehicle state

Return type

ndarray(3)

Predict the next state based on current state and odometry value. v is a random variable that represents additive odometry noise for simulation purposes.

\[f: \vec{x}_k, \delta_d, \delta_\theta \mapsto \vec{x}_{k+1}\]

For particle filters it is useful to apply the odometry to the states of all particles and this can be achieved if x is a 2D array with one state per row. v is ignored in this case.

Note

This is the state update equation used for EKF localization.

Seealso

deriv() Fx() Fv()

Fx(x, odo)[source]

Jacobian of state transition function df/dx

Parameters
  • x (array_like(3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

Returns

Jacobian matrix

Return type

ndarray(3,3)

Returns the Jacobian matrix \(\frac{\partial \vec{f}}{\partial \vec{x}}\) for the given state and odometry.

Seealso

f() deriv() Fv()

Fv(x, odo)[source]

Jacobian of state transition function df/dv

Parameters
  • x (array_like(3)) – vehicle state \((x, y, \theta)\)

  • odo (array_like(2)) – vehicle odometry \((\delta_d, \delta_\theta)\)

Returns

Jacobian matrix

Return type

ndarray(3,2)

Returns the Jacobian matrix \(\frac{\partial \vec{f}}{\partial \vec{v}}\) for the given state and odometry.

Seealso

f() deriv() Fx()

property control

Get/set vehicle control (superclass)

Getter

Returns vehicle’s control

Setter

Sets the vehicle’s control

Type

2-tuple, callable, interp1d or VehicleDriver

This is the input to the vehicle during a simulation performed by run(). The control input can be:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple

  • a driver agent, subclass of driversVehicleDriverBase

Example:

>>> from roboticstoolbox import Bicycle, RandomPath
>>> bike = Bicycle()
>>> bike.control = RandomPath(10)
>>> print(bike)
Bicycle: x = [ 0, 0, 0 ]
  L=1, steer_max=1.41372, speed_max=inf, accel_max=inf
Seealso

run() eval_control() scipy.interpolate.interp1d VehicleDriverBase

eval_control(control, x)[source]

Evaluate vehicle control input (superclass method)

Parameters
  • control ([type]) – vehicle control

  • x (array_like(3)) – vehicle state \((x, y, heta)\)

Raises

ValueError – bad control

Returns

vehicle control inputs

Return type

ndarray(2)

Evaluates the control for this time step and state. Control is set by the control() property to:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple

  • a driver agent, subclass of VehicleDriverBase

Vehicle steering, speed and acceleration limits are applied to the result before being input to the vehicle model.

Seealso

run() control() scipy.interpolate.interp1d()

limits_va(v)[source]

Apply velocity and acceleration limits (superclass)

Parameters

v (float) – desired velocity

Returns

allowed velocity

Return type

float

Determine allowable velocity given desired velocity, speed and acceleration limits.

Note

This function is stateful, requires previous velocity, _v_prev attribute, to enable acceleration limiting. This is reset to zero at the start of each simulation.

polygon(q)[source]

Bounding polygon at vehicle configuration

Parameters

q (array_like(3)) – vehicle configuration \((x, y, heta)\)

Returns

bounding polygon of vehicle at configuration q

Return type

Polygon2

The bounding polygon of the vehicle is returned for the configuration q. Can be used for collision checking using the intersects() method.

Seealso

Polygon2

abstract deriv(x, u)[source]
add_driver(driver)[source]

Add a driver agent (superclass)

Parameters

driver (VehicleDriverBase subclass) – a driver agent object

Warning

Deprecated. Use vehicle.control = driver instead.

Seealso

VehicleDriverBase control()

run(T=10, x0=None, control=None, animate=False)[source]

Simulate motion of vehicle (superclass)

Parameters
  • N (int, optional) – Number of simulation steps, defaults to 1000

  • x0 (array_like(3) or array_like(2)) – Initial state, defaults to value given to Vehicle constructor

  • animation (VehicleAnimation subclass, optional) – vehicle animation object, defaults to None

  • plot (bool, optional) – Enable plotting, defaults to False

Returns

State trajectory, each row is \((x,y,\theta)\).

Return type

ndarray(n,3)

Runs the vehicle simulation for N timesteps and optionally plots an animation. The method step() performs each time step.

The control inputs are providd by control which can be:

  • a constant tuple as the control inputs to the vehicle

  • a function called as f(vehicle, t, x) that returns a tuple

  • an interpolator called as f(t) that returns a tuple, see SciPy interp1d

  • a driver agent, subclass of VehicleDriverBase

This is evaluated by eval_control() which applies velocity, acceleration and steering limits.

The simulation can be stopped prematurely by the control function calling stopif().

Seealso

init() step() control()

init(x0=None, control=None)[source]

Initialize for simulation (superclass)

Parameters

x0 (array_like(3) or array_like(2)) – Initial state, defaults to value given to Vehicle constructor

Performs the following initializations:

  1. Clears the state history

  2. Set the private random number generator to initial value

  3. Sets state \(x = x_0\)

  4. Sets previous velocity to zero

  5. If a driver is attached, initialize it

  6. If animation is enabled, initialize that

If animation is enabled by constructor and no animation object is given, use a default VehiclePolygon("car")

Seealso

VehicleAnimationBase run()

step(u=None, animate=False)[source]

Step simulator by one time step (superclass)

Returns

odometry \((\delta_d, \delta_\theta)\)

Return type

ndarray(2)

  1. Obtain the vehicle control inputs

  2. Integrate the vehicle state forward one timestep

  3. Updates the stored state and state history

  4. Update animation if enabled.

  5. Returns the odometry

  • veh.step((vel, steer)) for a Bicycle vehicle model

  • veh.step((vel, vel_diff)) for a Unicycle vehicle model

  • veh.step() as above but control is taken from the control attribute which might be a function or driver agent.

Example:

>>> from roboticstoolbox import Bicycle
>>> bike = Bicycle()  # default bicycle model
>>> bike.step((1, 0.2))  # one step: v=1, γ=0.2
array([0.1   , 0.0203])
>>> bike.x
array([0.1   , 0.    , 0.0203])
>>> bike.step((1, 0.2))  # another step: v=1, γ=0.2
array([0.1   , 0.0203])
>>> bike.x
array([0.2   , 0.002 , 0.0405])

Note

Vehicle control input limits are applied.

Seealso

control(), update(), run()

property workspace

Size of robot workspace (superclass)

Returns

workspace bounds [xmin, xmax, ymin, ymax]

Return type

ndarray(4)

Returns the bounds of the workspace as specified by constructor option workspace

property x

Get vehicle state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Seealso

q()

property q

Get vehicle state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Seealso

x()

property x0

Get vehicle initial state/configuration (superclass)

Returns

Vehicle state \((x, y, \theta)\)

Return type

ndarray(3)

Set by VehicleBase subclass constructor. The state is set to this value at the beginning of each simulation run.

Seealso

run()

property random

Get private random number generator

Returns

NumPy random number generator

Return type

numpy.random.Generator

Has methods including:

The generator is initialized with the seed provided at constructor time every time init() is called.

Seealso

init() numpy.random.default_rng()

property x_hist

Get vehicle state/configuration history (superclass)

Returns

Vehicle state history

Return type

ndarray(n,3)

The state \((x, y, heta)\) at each time step resulting from a simulation run, one row per timestep.

Seealso

run()

property speed_max

Get maximum speed of vehicle (superclass)

Returns

maximum speed

Return type

float

Set by VehicleBase() subclass constructor.

Seealso

accel_max() steer_max() u_limited()

property accel_max

Get maximum acceleration of vehicle (superclass)

Returns

maximum acceleration

Return type

float

Set by VehicleBase() subclass constructor.

Seealso

speed_max() steer_max() u_limited()

property dt

Get sample time (superclass)

Returns

discrete time step for simulation

Return type

float

Set by VehicleBase() subclass constructor. Used by step() to update state.

Seealso

run()

property verbose

Get verbosity (superclass)

Returns

verbosity level

Return type

bool

Set by VehicleBase subclass constructor.

stopsim()[source]

Stop the simulation (superclass)

A control function can stop the simulation initated by the run method.

Seealso

run()

plot_xy(*args, block=False, **kwargs)[source]

Plot xy-path from history

Parameters
  • block (bool, optional) – block until plot dismissed, defaults to False

  • args – positional arguments passed to plot()

  • kwargs – keyword arguments passed to plot()

The \((x,y)\) trajectory from the simulation history is plotted as \(x\) vs :math:`y.

Seealso

run() plot_xyt()

plot_xyt(block=False, **kwargs)[source]

Plot configuration vs time from history

Parameters
  • block (bool, optional) – block until plot dismissed, defaults to False

  • args – positional arguments passed to plot()

  • kwargs – keyword arguments passed to plot()

The \((x,y, heta)\) trajectory from the simulation history is plotted against time.

Seealso

run() plot_xy()