Mobile robot kinematic models

Bicycle model

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

Create new 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 Vehicle constructor

Seealso

Vehicle

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

Seealso

curvature_max()

property curvature_max

Vehicle maximum path curature

Returns

maximum curvature

Return type

float

Seealso

radius_min()

property steer_max

Vehicle maximum steered wheel angle

Returns

maximum angle

Return type

float

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

Predict next state based on odometry

Parameters
  • x (array_like(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)

Returns the predicted 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, \vec{d}, \vec{v} \mapsto \vec{x}_{k+1}\]

Example:

>>> from roboticstoolbox import Bicycle
>>> bike = Bicycle()  # default bicycle model
>>> bike.f([0,0,0], [0.2, 0.1])
array([0.2, 0. , 0.1])

Note

This is the state update equation used for EKF localization.

Seealso

Fx() Fv()

Fx(x, odo)[source]

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

Seealso

Bicycle.f(), Bicycle.Fv()

Fv(x, odo)[source]

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

Seealso

Bicycle.f(), Bicycle.Fx()

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

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

u_limited(u)[source]
property accel_max

Get maximum acceleration of vehicle (superclass method)

Returns

maximum acceleration

Return type

float

Set by Vehicle subclass constructor.

add_driver(driver)

Add a driver agent (superclass method)

Parameters

driver (VehicleDriver subclass) – a driver agent object

Seealso

RandomPath()

property control

Get vehicle control (superclass method)

Returns

current control

Return type

2-tuple, callable, interp1d or VehicleDriver

Control 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 VehicleDriver()

Seealso

eval_control()

property dt

Get sample time (superclass method)

Returns

discrete time step for simulation

Return type

float

Set by Vehicle subclass constructor.

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 can be:

  • a constant 2-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 VehicleDriver subclass object

Note

Vehicle steering, speed and acceleration limits are applied.

init(x0=None, control=None)

Initialize for simulation (superclass method)

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. Sets state \(x = x_0\)

  3. If a driver is attached, initialize it

  4. If plotting is enabled, initialize that

If plot is set and no animation object is given, use a default VehiclePolygon('car')

Seealso

VehicleAnimation()

limits_va(v)

Apply velocity and acceleration limits (superclass method)

Parameters

v (float) – commanded velocity

Returns

allowed velocity

Return type

float

Note

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

path(t=10, u=None, x0=None)

Compute path by integration (superclass method)

param t

[description], defaults to None

type t

[type], optional

param u

[description], defaults to None

type u

[type], optional

param x0

initial state, defaults to (0,0,0)

type x0

array_like(3), optional

return

time vector and state history

rtype

(ndarray(1), ndarray(n,3))

% XF = V.path(TF, U) is the final state of the vehicle (3x1) from the initial

% state (0,0,0) with the control inputs U (vehicle specific). TF is a scalar to % specify the total integration time. % % XP = V.path(TV, U) is the trajectory of the vehicle (Nx3) from the initial % state (0,0,0) with the control inputs U (vehicle specific). T is a vector (N) of % times for which elements of the trajectory will be computed. % % XP = V.path(T, U, X0) as above but specify the initial state. % % Notes:: % - Integration is performed using ODE45. % - The ODE being integrated is given by the deriv method of the vehicle object.

# t, x = veh.path(5, u=control)

# print(t)

plot(x=None, shape='box', block=False, size=True, **kwargs)

[summary] (superclass method)

Parameters
  • path ([type], optional) – [description], defaults to None

  • block (bool, optional) – [description], defaults to True

plot_xy(*args, block=False, **kwargs)
plot_xyt(block=False, **kwargs)
polygon(q)
property q

Get vehicle state/configuration (superclass method)

Returns

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

Return type

ndarray(3)

property random

Get private random number generator

Returns

NumPy random number generator

Return type

Generator

Has methods including:

  • integers(low, high, size, endpoint)

  • random(size)

  • uniform

  • normal(mean, std, size)

  • multivariate_normal(mean, covar, size)

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

Seealso

init()

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

Simulate motion of vehicle (superclass method)

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, heta)\).

Return type

ndarray(n,3)

Runs the vehicle simulation for N timesteps and optionally plots an animation.

The control inputs are provied 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 VehicleDriver()

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 method)

Returns

maximum speed

Return type

float

Set by Vehicle subclass constructor.

step(u=None, animate=False)

Step simulator by one time step (superclass method)

Returns

odometry \((\delta_d, \delta_ heta)\)

Return type

ndarray(2)

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

  1. Integrates the vehicle forward one timestep

  2. Updates the stored state and state history

  3. Returns the odometry

Example:

ValueError: bad control specified
Traceback (most recent call last):
  File "<input>", line 1, in <module>
  File "/opt/hostedtoolcache/Python/3.7.11/x64/lib/python3.7/site-packages/roboticstoolbox/mobile/Vehicle.py", line 494, in step
    u = self.eval_control(u, self._x)
  File "/opt/hostedtoolcache/Python/3.7.11/x64/lib/python3.7/site-packages/roboticstoolbox/mobile/Vehicle.py", line 570, in eval_control
    raise ValueError('bad control specified')
ValueError: bad control specified

Note

Vehicle control input limits are applied.

Seealso

control(), update(), run()

stopif(stop)

Stop the simulation (superclass method)

Parameters

stop (bool) – stop condition

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

Seealso

run()

property verbose

Get verbosity (superclass method)

Returns

verbosity level

Return type

bool

Set by Vehicle subclass constructor.

property workspace

Size of robot workspace

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 method)

Returns

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

Return type

ndarray(3)

property x0

Get vehicle initial state/configuration (superclass method)

Returns

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

Return type

ndarray(3)

The state is set to this value at the beginning of each simulation run.

Set by Vehicle subclass constructor.

Seealso

run()

property x_hist

Get vehicle state/configuration history (superclass method)

Returns

Vehicle state history

Return type

ndarray(n,3)

The state at each time step resulting from a simulation run.

Seealso

run()

Unicycle model

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

Bases: roboticstoolbox.mobile.Vehicle.VehicleBase

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

Create new unicycle kinematic model

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

  • **kwargs

    additional arguments passed to Vehicle constructor

Seealso

Vehicle

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

Predict next state based on odometry

Parameters
  • x (array_like(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)

Returns the predicted 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, \vec{d}, \vec{v} \mapsto \vec{x}_{k+1}\]

Example:

TypeError: 'float' object cannot be interpreted as an integer

Note

This is the state update equation used for EKF localization.

Seealso

Fx() Fv()

Fx(x, odo)[source]

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

Seealso

Bicycle.f(), Bicycle.Fv()

Fv(x, odo)[source]

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

Seealso

Bicycle.f(), Bicycle.Fx()

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

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

Note

Vehicle speed and steering limits are not applied here

u_limited(u)[source]
property accel_max

Get maximum acceleration of vehicle (superclass method)

Returns

maximum acceleration

Return type

float

Set by Vehicle subclass constructor.

add_driver(driver)

Add a driver agent (superclass method)

Parameters

driver (VehicleDriver subclass) – a driver agent object

Seealso

RandomPath()

property control

Get vehicle control (superclass method)

Returns

current control

Return type

2-tuple, callable, interp1d or VehicleDriver

Control 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 VehicleDriver()

Seealso

eval_control()

property dt

Get sample time (superclass method)

Returns

discrete time step for simulation

Return type

float

Set by Vehicle subclass constructor.

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 can be:

  • a constant 2-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 VehicleDriver subclass object

Note

Vehicle steering, speed and acceleration limits are applied.

init(x0=None, control=None)

Initialize for simulation (superclass method)

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. Sets state \(x = x_0\)

  3. If a driver is attached, initialize it

  4. If plotting is enabled, initialize that

If plot is set and no animation object is given, use a default VehiclePolygon('car')

Seealso

VehicleAnimation()

limits_va(v)

Apply velocity and acceleration limits (superclass method)

Parameters

v (float) – commanded velocity

Returns

allowed velocity

Return type

float

Note

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

path(t=10, u=None, x0=None)

Compute path by integration (superclass method)

param t

[description], defaults to None

type t

[type], optional

param u

[description], defaults to None

type u

[type], optional

param x0

initial state, defaults to (0,0,0)

type x0

array_like(3), optional

return

time vector and state history

rtype

(ndarray(1), ndarray(n,3))

% XF = V.path(TF, U) is the final state of the vehicle (3x1) from the initial

% state (0,0,0) with the control inputs U (vehicle specific). TF is a scalar to % specify the total integration time. % % XP = V.path(TV, U) is the trajectory of the vehicle (Nx3) from the initial % state (0,0,0) with the control inputs U (vehicle specific). T is a vector (N) of % times for which elements of the trajectory will be computed. % % XP = V.path(T, U, X0) as above but specify the initial state. % % Notes:: % - Integration is performed using ODE45. % - The ODE being integrated is given by the deriv method of the vehicle object.

# t, x = veh.path(5, u=control)

# print(t)

plot(x=None, shape='box', block=False, size=True, **kwargs)

[summary] (superclass method)

Parameters
  • path ([type], optional) – [description], defaults to None

  • block (bool, optional) – [description], defaults to True

plot_xy(*args, block=False, **kwargs)
plot_xyt(block=False, **kwargs)
polygon(q)
property q

Get vehicle state/configuration (superclass method)

Returns

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

Return type

ndarray(3)

property random

Get private random number generator

Returns

NumPy random number generator

Return type

Generator

Has methods including:

  • integers(low, high, size, endpoint)

  • random(size)

  • uniform

  • normal(mean, std, size)

  • multivariate_normal(mean, covar, size)

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

Seealso

init()

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

Simulate motion of vehicle (superclass method)

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, heta)\).

Return type

ndarray(n,3)

Runs the vehicle simulation for N timesteps and optionally plots an animation.

The control inputs are provied 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 VehicleDriver()

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 method)

Returns

maximum speed

Return type

float

Set by Vehicle subclass constructor.

step(u=None, animate=False)

Step simulator by one time step (superclass method)

Returns

odometry \((\delta_d, \delta_ heta)\)

Return type

ndarray(2)

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

  1. Integrates the vehicle forward one timestep

  2. Updates the stored state and state history

  3. Returns the odometry

Example:

ValueError: bad control specified
Traceback (most recent call last):
  File "<input>", line 1, in <module>
  File "/opt/hostedtoolcache/Python/3.7.11/x64/lib/python3.7/site-packages/roboticstoolbox/mobile/Vehicle.py", line 494, in step
    u = self.eval_control(u, self._x)
  File "/opt/hostedtoolcache/Python/3.7.11/x64/lib/python3.7/site-packages/roboticstoolbox/mobile/Vehicle.py", line 570, in eval_control
    raise ValueError('bad control specified')
ValueError: bad control specified

Note

Vehicle control input limits are applied.

Seealso

control(), update(), run()

stopif(stop)

Stop the simulation (superclass method)

Parameters

stop (bool) – stop condition

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

Seealso

run()

property verbose

Get verbosity (superclass method)

Returns

verbosity level

Return type

bool

Set by Vehicle subclass constructor.

property workspace

Size of robot workspace

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 method)

Returns

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

Return type

ndarray(3)

property x0

Get vehicle initial state/configuration (superclass method)

Returns

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

Return type

ndarray(3)

The state is set to this value at the beginning of each simulation run.

Set by Vehicle subclass constructor.

Seealso

run()

property x_hist

Get vehicle state/configuration history (superclass method)

Returns

Vehicle state history

Return type

ndarray(n,3)

The state at each time step resulting from a simulation run.

Seealso

run()

Superclass

roboticstoolbox.mobile.Vehicle

alias of <module ‘roboticstoolbox.mobile.Vehicle’ from ‘/opt/hostedtoolcache/Python/3.7.11/x64/lib/python3.7/site-packages/roboticstoolbox/mobile/Vehicle.py’>