from typing import Type
import numpy as np
from math import sin, cos, atan2, tan, sqrt, pi
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon
import time
from bdsim.components import TransferBlock
from bdsim.graphics import GraphicsBlock
from spatialmath import base
from roboticstoolbox import mobile
# ------------------------------------------------------------------------ #
[docs]class Bicycle(TransferBlock):
"""
:blockname:`BICYCLE`
.. table::
:align: left
+------------+------------+---------+
| inputs | outputs | states |
+------------+------------+---------+
| 2 | 1 | 3 |
+------------+------------+---------+
| float | ndarray(3) | |
+------------+------------+---------+
"""
nin = 2
nout = 1
inlabels = ('v', 'γ')
outlabels = ('q',)
[docs] def __init__(self, L=1, speed_max=np.inf, accel_max=np.inf, steer_max=0.45 * pi,
x0=None, **blockargs):
r"""
Create a vehicle model with Bicycle kinematics.
:param L: Wheelbase, defaults to 1
:type L: float, optional
:param speed_max: Velocity limit, defaults to 1
:type speed_max: float, optional
:param accel_max: maximum acceleration, defaults to math.inf
:type accel_max: float, optional
:param steer_max: maximum steering angle, defaults to math.pi*0.45
:type steer_max: float, optional
:param x0: Inital state, defaults to None
:type x0: array_like, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a BICYCLE block
:rtype: Bicycle instance
Bicycle kinematic model with state :math:`[x, y, \theta]`.
**Block ports**
:input v: Vehicle speed (metres/sec). The velocity limit ``vlim`` is
applied to the magnitude of this input.
:input γ: Steering wheel angle (radians). The steering limit ``slim``
is applied to the magnitude of this input.
:output q: configuration (x, y, θ)
:seealso: :class:`mobile.Bicycle` :class:`DiffSteer`
"""
# TODO: add option to model the effect of steering arms, responds to
# gamma dot
super().__init__(nstates=3, **blockargs)
self.vehicle = mobile.Bicycle(L=L,
steer_max=steer_max, speed_max=speed_max, accel_max=accel_max)
if x0 is None:
self._x0 = np.zeros((self.nstates,))
else:
assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates)
self._x0 = x0
self.inport_names(('v', '$\gamma$'))
self.outport_names(('q',))
self.state_names(('x', 'y', r'$\theta$'))
def output(self, t):
return [self._x] # one output which is ndarray(3)
def deriv(self):
return self.vehicle.deriv(self._x, self.inputs)
# ------------------------------------------------------------------------ #
[docs]class Unicycle(TransferBlock):
"""
:blockname:`UNICYCLE`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 2 | 1 | 3 |
+------------+---------+---------+
| float | float | |
+------------+---------+---------+
"""
nin = 2
nout = 1
inlabels = ('v', 'ω')
outlabels = ('q',)
[docs] def __init__(self, w=1, speed_max=np.inf, accel_max=np.inf, steer_max=None,
a=0, x0=None, **blockargs):
r"""
Create a vehicle model with Unicycle kinematics.
:param w: vehicle width, defaults to 1
:type w: float, optional
:param speed_max: Velocity limit, defaults to 1
:type speed_max: float, optional
:param accel_max: maximum acceleration, defaults to math.inf
:type accel_max: float, optional
:param steer_max: maximum steering rate, defaults to 1
:type steer_max: float, optional
:param x0: Inital state, defaults to None
:type x0: array_like, optional
:param blockargs: |BlockOptions|
:type blockargs: dict :return: a UNICYCLE block
:rtype: Unicycle instance
Unicycle kinematic model with state :math:`[x, y, \theta]`.
**Block ports**
:input v: Vehicle speed (metres/sec). The velocity limit ``vlim`` is
applied to the magnitude of this input.
:input ω: Angular velocity (radians/sec). The steering limit ``slim``
is applied to the magnitude of this input.
:output q: configuration (x, y, θ)
:seealso: :class:`Bicycle` :class:`DiffSteer`
"""
super().__init__(nstates=3, **blockargs)
if x0 is None:
self._x0 = np.zeros((self.nstates,))
else:
assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates)
self._x0 = x0
self.vehicle = mobile.Unicycle(w=w,
steer_max=steer_max, speed_max=speed_max, accel_max=accel_max)
#TODO, add support for origin shift
# If ``a`` is non-zero then the planar velocity of that point $x=a$
# can be controlled by
# .. math::
# \begin{pmatrix} v \\ \omega \end{pmatrix} =
# \begin{pmatrix}
# \cos \theta & \sin \theta \\
# -\frac{1}{a}\sin \theta & \frac{1}{a}\cos \theta
# \end{pmatrix}\begin{pmatrix}
# \dot{x} \\ \dot{y}
# \end{pmatrix}
def output(self, t):
return self._x
def deriv(self):
return self.vehicle.deriv(self._x, self.inputs)
# ------------------------------------------------------------------------ #
[docs]class DiffSteer(TransferBlock):
"""
:blockname:`DIFFSTEER`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 2 | 3 | 3 |
+------------+---------+---------+
| float | float | |
+------------+---------+---------+
"""
nin = 2
nout = 1
inlabels = ('ωL', 'ωR')
outlabels = ('q',)
[docs] def __init__(self, w=1, R=1, speed_max=np.inf, accel_max=np.inf, steer_max=None,
a=0, x0=None, **blockargs):
"""
Create a differential steer vehicle model
:param w: vehicle width, defaults to 1
:type w: float, optional
:param R: Wheel radius, defaults to 1
:type R: float, optional
:param speed_max: Velocity limit, defaults to 1
:type speed_max: float, optional
:param accel_max: maximum acceleration, defaults to math.inf
:type accel_max: float, optional
:param steer_max: maximum steering rate, defaults to 1
:type steer_max: float, optional
:param x0: Inital state, defaults to None
:type x0: array_like, optional
:param blockargs: |BlockOptions|
:type blockargs: dict :return: a DIFFSTEER block
:rtype: DifSteer instance
Unicycle kinematic model with state :math:`[x, y, \theta]`, with
with inputs given as wheel angular velocity.
**Block ports**
:input ωL: Left-wheel angular velocity (radians/sec).
:input ωR: Right-wheel angular velocity (radians/sec).
:output q: configuration (x, y, θ)
.. note:: Wheel velocity is defined such that if both are positive the vehicle
moves forward.
:seealso: :class:`Bicycle` :class:`Unicycle`
"""
super().__init__(nstates=3, **blockargs)
self.type = 'diffsteer'
self.R = R
if x0 is None:
self._x0 = np.zeros((slef.nstates,))
else:
assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates)
self._x0 = x0
self.vehicle = mobile.Unicycle(w=w,
steer_max=steer_max, speed_max=speed_max, accel_max=accel_max)
def output(self, t):
return self._x
def deriv(self):
# compute (v, omega) from left/right wheel speeds
v = self.R * (self.inputs[0] + self.inputs[1]) / 2
omega = (self.inputs[1] + self.inputs[0]) / self.W
return self.vehicle.deriv(self._x, (v, omega))
# ------------------------------------------------------------------------ #
[docs]class VehiclePlot(GraphicsBlock):
"""
:blockname:`VEHICLEPLOT`
.. table::
:align: left
+--------+---------+---------+
| inputs | outputs | states |
+--------+---------+---------+
| 1 | 0 | 0 |
+--------+---------+---------+
| ndarray| | |
+--------+---------+---------+
"""
nin = 1
nout = 0
inlabels = ('q',)
# TODO add ability to render an image instead of an outline
[docs] def __init__(self, animation=None, path=None, labels=['X', 'Y'], square=True, init=None, scale=True, **blockargs):
"""
Create a vehicle animation
:param animation: Graphical animation of vehicle, defaults to None
:type animation: VehicleAnimation subclass, optional
:param path: linestyle to plot path taken by vehicle, defaults to None
:type path: str or dict, optional
:param labels: axis labels (xlabel, ylabel), defaults to ["X","Y"]
:type labels: array_like(2) or list
:param square: Set aspect ratio to 1, defaults to True
:type square: bool, optional
:param init: initialize graphics, defaults to None
:type init: callable, optional
:param blockargs: |BlockOptions|
:type blockargs: dict :return: A VEHICLEPLOT block
:rtype: VehiclePlot instance
Create a vehicle animation similar to the figure below.
**Block ports**
:input q: configuration (x, y, θ)
Notes:
- The ``init`` function is called after the axes are initialized
and can be used to draw application specific detail on the
plot. In the example below, this is the dot and star.
- A dynamic trail, showing path to date can be animated if
the option ``path`` is set to a linestyle.
.. figure:: ../../figs/rvc4_4.gif
:width: 500px
:alt: example of generated graphic
Example of vehicle display (animated). The label at the top is the
block name.
"""
super().__init__(**blockargs)
self.xdata = []
self.ydata = []
self.type = 'vehicleplot'
if init is not None:
assert callable(init), 'graphics init function must be callable'
self.init = init
self.square = square
self.pathstyle = path
if scale != 'auto':
if len(scale) == 2:
scale = scale * 2
self.scale = scale
self.labels = labels
if animation is None:
animation = mobile.VehiclePolygon()
elif not isinstance(animation, mobile.VehicleAnimationBase):
raise TypeError('animation object must be VehicleAnimationBase subclass')
self.animation = animation
def start(self, state=None):
# create the plot
super().reset()
try:
print('graphics start')
self.fig = self.create_figure(state)
print('fig created')
self.ax = self.fig.gca()
print('axes')
except:
print('aaargh')
if self.square:
self.ax.set_aspect('equal')
print('done')
self.ax.grid(True)
self.ax.set_xlabel(self.labels[0])
self.ax.set_ylabel(self.labels[1])
self.ax.set_title(self.name)
if self.scale != 'auto':
self.ax.set_xlim(*self.scale[0:2])
self.ax.set_ylim(*self.scale[2:4])
if self.init is not None:
self.init(self.ax)
if isinstance(self.pathstyle, str):
self.line, = plt.plot(0, 0, self.pathstyle)
elif isinstance(self.pathstyle, dict):
self.line, = plt.plot(0, 0, **self.pathstyle)
self.animation.add()
plt.draw()
plt.show(block=False)
super().start()
def step(self, state=None, **kwargs):
# inputs are set
xyt = self.inputs[0]
# update the path line
self.xdata.append(xyt[0])
self.ydata.append(xyt[1])
#plt.figure(self.fig.number)
if self.pathstyle is not None:
self.line.set_data(self.xdata, self.ydata)
# update the vehicle pose
self.animation.update(xyt)
if self.scale == 'auto':
self.ax.relim()
self.ax.autoscale_view()
super().step(state=state)
def done(self, block=False, **kwargs):
if self.bd.options.graphics:
plt.show(block=block)
super().done()