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.
- 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
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.
- 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.
- 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 tuplean interpolator called as
f(t)
that returns a tuplea 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
- property dt
Get sample time (superclass)
- Returns
discrete time step for simulation
- Return type
float
Set by
VehicleBase()
subclass constructor. Used bystep()
to update state.- Seealso
- 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 tuplean 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.
- 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.
- 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:
Clears the state history
Set the private random number generator to initial value
Sets state \(x = x_0\)
Sets previous velocity to zero
If a driver is attached, initialize it
If animation is enabled, initialize that
If animation is enabled by constructor and no animation object is given, use a default
VehiclePolygon("car")
- Seealso
- 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
The \((x,y)\) trajectory from the simulation history is plotted as \(x\) vs :math:`y.
- Seealso
- plot_xyt(block=False, **kwargs)
Plot configuration vs time from history
- Parameters
The \((x,y, heta)\) trajectory from the simulation history is plotted against time.
- 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 theintersects()
method.- Seealso
Polygon2
- property q
Get vehicle state/configuration (superclass)
- Returns
Vehicle state \((x, y, \theta)\)
- Return type
ndarray(3)
- Seealso
- property random
Get private random number generator
- Returns
NumPy random number generator
- Return type
Has methods including:
The generator is initialized with the seed provided at constructor time every time
init()
is called.- Seealso
- 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 methodstep()
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 tuplean interpolator called as
f(t)
that returns a tuple, see SciPy interp1da 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()
.
- 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)
Obtain the vehicle control inputs
Integrate the vehicle state forward one timestep
Updates the stored state and state history
Update animation if enabled.
Returns the odometry
veh.step((vel, steer))
for a Bicycle vehicle modelveh.step((vel, vel_diff))
for a Unicycle vehicle modelveh.step()
as above but control is taken from thecontrol
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.
- stopsim()
Stop the simulation (superclass)
A control function can stop the simulation initated by the
run
method.- Seealso
- 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
- 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