def output(self, t=None):
model = self.model
# compute output vector as a function of state vector
# z Position 3x1 (x,y,z)
# v Velocity 3x1 (xd,yd,zd)
# n Attitude 3x1 (Y,P,R)
# o Angular velocity 3x1 (Yd,Pd,Rd)
n = self._x[3:6] # RPY angles
phi = n[0] # yaw
the = n[1] # pitch
psi = n[2] # roll
# rotz(phi)*roty(the)*rotx(psi)
# BBF > Inertial rotation matrix
R = np.array([[
cos(the) * cos(phi),
sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi),
cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)
],
[
cos(the) * sin(phi),
sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi),
cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi)
], [-sin(the),
sin(psi) * cos(the),
cos(psi) * cos(the)]])
# inverted Wronskian
iW = np.array([[0, sin(psi), cos(psi)],
[0, cos(psi) * cos(the), -sin(psi) * cos(the)],
[cos(the),
sin(psi) * sin(the),
cos(psi) * sin(the)]]) / cos(the)
# return velocity in the body frame
out = {}
out['x'] = self._x[0:6]
# translational velocity mapped to body frame
out['vb'] = np.linalg.inv(R) @ self._x[6:9]
# RPY rates mapped to body frame
out['w'] = iW @ self._x[9:12]
out['a1s'] = self.a1s
out['b1s'] = self.b1s
return [out]
def getstate(self):
# get the state from each stateful block
x0 = np.array([])
for b in self.blocklist:
if isinstance(b, TransferBlock):
x0 = np.r_[x0, b.getstate()]
# print('x0', x0)
return x0
def evaluate(self, x, t):
"""
Evaluate all blocks in the network
:param x: state
:type x: numpy.ndarray
:param t: current time
:type t: float
:return: state derivative
:rtype: numpy.ndarray
Performs the following steps:
1. Partition the state vector to all stateful blocks
2. Propogate known block output ports to connected input ports
"""
# print('in evaluate at t=', t)
self.t = t
DEBUG('state', '>>>>>>>>> t=', t, ', x=', x, '>>>>>>>>>>>>>>>>')
# reset all the blocks ready for the evalation
self.reset()
# split the state vector to stateful blocks
for b in self.blocklist:
if isinstance(b, TransferBlock):
x = b.setstate(x)
# process blocks with initial outputs and propagate
for b in self.blocklist:
if isinstance(b, (SourceBlock, TransferBlock)):
self._propagate(b, t)
# check we have values for all
for b in self.blocklist:
if b.nin > 0 and not b.done:
raise RuntimeError(str(b) + ' has incomplete inputs')
# gather the derivative
YD = np.array([])
for b in self.blocklist:
if isinstance(b, TransferBlock):
assert b.updated, str(b) + ' has incomplete inputs'
yd = b.deriv().flatten()
YD = np.r_[YD, yd]
DEBUG('deriv', YD)
return YD
def __init__(self, value=None, **kwargs):
"""
:param value: the constant, defaults to None
:type value: any
:param ``**kwargs``: common Block options
:return: a CONSTANT block
:rtype: Constant instance
Create a constant block.
This block has only one output port, but the value can be any
Python type, so long as the connected input port can handle it.
For example float, list or numpy ndarray.
"""
super().__init__(nout=1, **kwargs)
if isinstance(value, (tuple, list)):
value = np.array(value)
self.value = value
self.type = 'constant'
def __init__(self,
N=1,
D=[1, 1],
*inputs,
x0=None,
verbose=False,
**kwargs):
r"""
:param N: numerator coefficients, defaults to 1
:type N: array_like, optional
:param D: denominator coefficients, defaults to [1, 1]
:type D: array_like, optional
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param x0: initial states, defaults to zero
:type x0: array_like, optional
:param ``**kwargs``: common Block options
:return: A SCOPE block
:rtype: LTI_SISO instance
Create a SISO LTI block.
Describes the dynamics of a single-input single-output (SISO) linear
time invariant (LTI) system described by numerator and denominator
polynomial coefficients.
Coefficients are given in the order from highest order to zeroth
order, ie. :math:`2s^2 - 4s +3` is ``[2, -4, 3]``.
Only proper transfer functions, where order of numerator is less
than denominator are allowed.
The order of the states in ``x0`` is consistent with controller canonical
form.
Examples::
LTI_SISO(N=[1, 2], D=[2, 3, -4])
is the transfer function :math:`\frac{s+2}{2s^2+3s-4}`.
"""
#print('in SISO constscutor')
if not isinstance(N, list):
N = [N]
if not isinstance(D, list):
D = [D]
self.N = N
self.D = N
n = len(D) - 1
nn = len(N)
if x0 is None:
x0 = np.zeros((n, ))
assert nn <= n, 'direct pass through is not supported'
# convert to numpy arrays
N = np.r_[np.zeros((len(D) - len(N), )), np.array(N)]
D = np.array(D)
# normalize the coefficients to obtain
#
# b_0 s^n + b_1 s^(n-1) + ... + b_n
# ---------------------------------
# a_0 s^n + a_1 s^(n-1) + ....+ a_n
# normalize so leading coefficient of denominator is one
D0 = D[0]
D = D / D0
N = N / D0
A = np.eye(len(D) - 1, k=1) # control canonic (companion matrix) form
A[-1, :] = -D[1:]
B = np.zeros((n, 1))
B[-1] = 1
C = (N[1:] - N[0] * D[1:]).reshape((1, n))
if verbose:
print('A=', A)
print('B=', B)
print('C=', C)
super().__init__(A=A, B=B, C=C, x0=x0, **kwargs)
self.type = 'LTI'
def deriv(self):
return self.A @ self._x + self.B @ np.array(self.inputs)
def deriv(self):
xd = np.array(self.inputs)
for i in range(0, self.nstates):
if self._x[i] < self.min[i] or self._x[i] > self.max[i]:
xd[i] = 0
return xd
def deriv(self):
model = self.model
# Body-fixed frame references
# ei Body fixed frame references 3x1
e3 = np.r_[0, 0, 1]
# process inputs
w = self.inputs[0]
if len(w) != self.nrotors:
raise RuntimeError('input vector wrong size')
if self.speedcheck and np.any(w == 0):
# might need to fix this, preculudes aerobatics :(
# mu becomes NaN due to 0/0
raise RuntimeError(
'quadrotor_dynamics: not defined for zero rotor speed')
# EXTRACT STATES FROM X
z = self._x[0:3] # position in {W}
n = self._x[3:6] # RPY angles {W}
v = self._x[6:9] # velocity in {W}
o = self._x[9:12] # angular velocity in {W}
# PREPROCESS ROTATION AND WRONSKIAN MATRICIES
phi = n[0] # yaw
the = n[1] # pitch
psi = n[2] # roll
# rotz(phi)*roty(the)*rotx(psi)
# BBF > Inertial rotation matrix
R = np.array([[
cos(the) * cos(phi),
sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi),
cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)
],
[
cos(the) * sin(phi),
sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi),
cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi)
], [-sin(the),
sin(psi) * cos(the),
cos(psi) * cos(the)]])
# Manual Construction
# Q3 = [cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1]; % RZ %Rotation mappings
# Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; % RY
# Q1 = [1 0 0;0 cos(psi) -sin(psi);0 sin(psi) cos(psi)]; % RX
# R = Q3*Q2*Q1 %Rotation matrix
#
# RZ * RY * RX
# inverted Wronskian
iW = np.array([[0, sin(psi), cos(psi)],
[0, cos(psi) * cos(the), -sin(psi) * cos(the)],
[cos(the),
sin(psi) * sin(the),
cos(psi) * sin(the)]]) / cos(the)
# ROTOR MODEL
T = np.zeros((3, 4))
Q = np.zeros((3, 4))
tau = np.zeros((3, 4))
a1s = self.a1s
b1s = self.b1s
for i in range(0, self.nrotors): # for each rotor
# Relative motion
Vr = np.cross(o, self.D[:, i]) + v
# Magnitude of mu, planar components
mu = sqrt(np.sum(Vr[0:2]**2)) / (abs(w[i]) * model['r'])
# Non-dimensionalised normal inflow
lc = Vr[2] / (abs(w[i]) * model['r'])
# Non-dimensionalised induced velocity approximation
li = mu
alphas = atan2(lc, mu)
# Sideslip azimuth relative to e1 (zero over nose)
j = atan2(Vr[1], Vr[0])
J = np.array([[cos(j), -sin(j)],
[sin(j),
cos(j)]]) # BBF > mu sideslip rotation matrix
# Flapping
beta = np.array([
[((8 / 3 * model['theta0'] + 2 * model['theta1']) * mu -
2 * lc * mu) / (1 - mu**2 / 2)], # Longitudinal flapping
# Lattitudinal flapping (note sign)
[0]
])
# sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)];
# Rotate the beta flapping angles to longitudinal and lateral coordinates.
beta = J.T @ beta
a1s[i] = beta[0] - 16 / model['gamma'] / abs(w[i]) * o[1]
b1s[i] = beta[1] - 16 / model['gamma'] / abs(w[i]) * o[0]
# Forces and torques
# Rotor thrust, linearised angle approximations
T[:, i] = model['Ct'] * model['rho'] * model['A'] * model['r']**2 * w[i]**2 * \
np.r_[-cos(b1s[i]) * sin(a1s[i]),
sin(b1s[i]), -cos(a1s[i])*cos(b1s[i])]
# Rotor drag torque - note that this preserves w[i] direction sign
Q[:, i] = -model['Cq'] * model['rho'] * \
model['A'] * model['r']**3 * w[i] * abs(w[i]) * e3
# Torque due to rotor thrust
tau[:, i] = np.cross(T[:, i], self.D[:, i])
# RIGID BODY DYNAMIC MODEL
dz = v
dn = iW @ o
dv = model['g'] * e3 + R @ np.sum(T, axis=1) / model['M']
# vehicle can't fall below ground, remember z is down
if self.groundcheck and z[2] > 0:
z[0] = 0
dz[0] = 0
# row sum of torques
do = np.linalg.inv(model['J']) @ (np.cross(
-o, model['J'] @ o) + np.sum(tau, axis=1) + np.sum(Q, axis=1))
# stash the flapping information for plotting
self.a1s = a1s
self.b1s = b1s
return np.r_[dz, dn, dv, do] # This is the state derivative vector
def simulate(bd: BlockDiagram,
T=10.0,
dt=0.1,
solver='RK45',
block=False,
checkfinite=True,
watch=[],
**kwargs):
"""
Run the block diagram
:param T: maximum integration time, defaults to 10.0
:type T: float, optional
:param dt: maximum time step, defaults to 0.1
:type dt: float, optional
:param solver: integration method, defaults to ``RK45``
:type solver: str, optional
:param block: matplotlib block at end of run, default False
:type block: bool
:param checkfinite: error if inf or nan on any wire, default True
:type checkfinite: bool
:param watch: list of input ports to log
:type watch: list
:param ``**kwargs``: passed to ``scipy.integrate``
:return: time history of signals and states
:rtype: Sim class
Assumes that the network has been compiled.
Graphics display in all blocks can be disabled using the `graphics`
option to the ``BlockDiagram`` instance.
Results are returned in a class with attributes:
- ``t`` the time vector: ndarray, shape=(M,)
- ``x`` is the state vector: ndarray, shape=(M,N)
- ``xnames`` is a list of the names of the states corresponding to columns of `x`, eg. "plant.x0",
defined for the block using the ``snames`` argument
- ``uN'` for a watched input where N is the index of the port mentioned in the ``watch`` argument
- ``unames`` is a list of the names of the input ports being watched, same order as in ``watch`` argument
If there are no dynamic elements in the diagram, ie. no states, then ``x`` and ``xnames`` are not
present.
The ``watch`` argument is a list of one or more input ports whose value during simulation
will be recorded. The elements of the list can be:
- a ``Block`` reference, which is interpretted as input port 0
- a ``Plug`` reference, ie. a block with an index or attribute
- a string of the form "block[i]" which is port i of the block named block.
"""
assert bd.compiled, 'Network has not been compiled'
bd.T = T
bd.count = 0
bd.stop = None # allow any block to stop.BlockDiagram by setting this to the block's name
bd.checkfinite = checkfinite
# preproces the watchlist
pluglist = []
plugnamelist = []
re_block = re.compile(r'(?P<name>[^[]+)(\[(?P<port>[0-9]+)\])')
for n in watch:
if isinstance(n, str):
# a name was given, with optional port number
m = re_block.match(n)
name = m.group('name')
port = m.group('port')
b = bd.blocknames[name]
plug = b[port]
elif isinstance(n, Block):
# a block was given, defaults to port 0
plug = n[0]
elif isinstance(n, Plug):
# a plug was given
plug = n
else:
raise Exception("unreachable")
pluglist.append(plug)
plugnamelist.append(str(plug))
# try:
# tell all blocks we're doing a.BlockDiagram
bd.start()
# get initial state from the stateful blocks
x0 = bd.getstate()
if len(x0) > 0:
print('initial state x0 = ', x0)
if bd.options.progress:
printProgressBar(0, prefix='Progress:', suffix='complete', length=60)
# out = scipy.integrate.solve_ivp.BlockDiagram._deriv, args=(bd,), t_span=(0,T), y0=x0,
# method=solver, t_eval=np.linspace(0, T, 100), events=None, **kwargs)
if len(x0) > 0:
# block diagram contains states, solve it using numerical integration
scipy_integrator = integrate.__dict__[
solver] # get user specified integrator
integrator = scipy_integrator(lambda t, y: bd.evaluate(y, t),
t0=0.0,
y0=x0,
t_bound=T,
max_step=dt)
# initialize list of time and states
tlist = []
xlist = []
plist = [[] for p in pluglist]
while integrator.status == 'running':
# step the integrator, calls _deriv multiple times
integrator.step()
if integrator.status == 'failed':
print('integration completed with failed status ')
# stash the results
tlist.append(integrator.t)
xlist.append(integrator.y)
# record the ports on the watchlist
for i, p in enumerate(pluglist):
plist[i].append(p.block.inputs[p.port])
# update all blocks that need to know
bd.step()
# update the progress bar
if bd.options.progress:
printProgressBar(integrator.t / T,
prefix='Progress:',
suffix='complete',
length=60)
# has any block called a stop?
if bd.stop is not None:
print('\n--- stop requested at t={:f} by {:s}'.format(
bd.t, str(bd.stop)))
break
# save buffered data in a Struct
out = Struct('results')
out.t = np.array(tlist)
out.x = np.array(xlist)
out.xnames = bd.statenames
for i, p in enumerate(pluglist):
out['u' + str(i)] = np.array(plist[i])
out.unames = plugnamelist
else:
# block diagram has no states
# initialize list of time and states
tlist = []
plist = [[] for p in pluglist]
for t in np.arange(0, T, dt): # step through the time range
# evaluate the block diagram
bd.evaluate([], t)
# stash the results
tlist.append(t)
# record the ports on the watchlist
for i, p in enumerate(pluglist):
plist[i].append(p.block.inputs[p.port])
# update all blocks that need to know
bd.step()
# update the progress bar
if bd.options.progress:
printProgressBar(t / T,
prefix='Progress:',
suffix='complete',
length=60)
# has any block called a stop?
if bd.stop is not None:
print('\n--- stop requested at t={:f} by {:s}'.format(
bd.t, str(bd.stop)))
break
# save buffered data in a Struct
out = Struct('results')
out.t = np.array(tlist)
for i, p in enumerate(pluglist):
out['u' + str(i)] = np.array(plist[i])
out.unames = plugnamelist
if bd.options.progress:
print('\r' + ' ' * 90 + '\r')
# except RuntimeError as err:
# # bad things happens, print a message and return no result
# print('unrecoverable error in evaluation: ', err)
# return None
# pause until all graphics blocks close
bd.done(block=block)
# print(bd.count, ' integrator steps')
return out
def __init__(self, *inputs, x=None, y=None, xy=None, time=False, kind='linear', **kwargs):
"""
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param x: x-values of function
:type x: array_like, shape (N,) optional
:param y: y-values of function
:type y: array_like, optional
:param xy: combined x- and y-values of function
:type xy: array_like, optional
:param time: x new is simulation time, defaults to False
:type time: bool
:param kind: interpolation method, defaults to 'linear'
:type kind: str
:param ``**kwargs``: common Block options
:return: INTERPOLATE block
:rtype: _Function
Create an interpolation block.
A block that interpolates its input according to a piecewise function.
A simple triangle function with domain [0,10] and range [0,1] can be
defined by::
INTERPOLATE(x=(0,5,10), y=(0,1,0))
We might also express this as::
INTERPOLATE(xy=[(0,0), (5,1), (10,0)])
The data can also be expressed as numpy arrays. If that is the case,
the interpolation function can be vector valued. ``x`` has a shape of
(N,1) and ``y`` has a shape of (N,M). Alternatively ``xy`` has a shape
of (N,M+1) and the first column is the x-data.
The input to the interpolator comes from:
- Input port 0
- Simulation time, if ``time=True``. In this case the block has no
input ports and is a ``Source`` not a ``Function``.
"""
self.time = time
if time:
nin = 0
self.blockclass = 'source'
else:
nin = 1
super().__init__(nin=nin, nout=1, inputs=inputs, **kwargs)
if xy is None:
# process separate x and y vectors
x = np.array(x)
y = np.array(y)
assert x.shape[0] == y.shape[0], 'x and y data must be same length'
else:
# process mixed xy data
if isinstance(xy, (list, tuple)):
x = [_[0] for _ in xy]
y = [_[1] for _ in xy]
# x = np.array(x).T
# y = np.array(y).T
print(x, y)
elif isinstance(xy, np.ndarray):
x = xy[:, 0]
y = xy[:, 1:]
self.f = scipy.interpolate.interp1d(
x=x, y=y, kind=kind, axis=0, **kwargs)
self.x = x
def step(self):
def plot3(h, x, y, z):
h.set_data(x, y)
h.set_3d_properties(z)
# READ STATE
z = self.inputs[0][0:3]
n = self.inputs[0][3:6]
# TODO, check input dimensions, 12 or 12+2N, deal with flapping
a1s = self.a1s
b1s = self.b1s
quad = self.model
# vehicle dimensons
d = quad['d'] # Hub displacement from COG
r = quad['r'] # Rotor radius
# PREPROCESS ROTATION MATRIX
phi = n[0] # Euler angles
the = n[1]
psi = n[2]
# BBF > Inertial rotation matrix
R = np.array([[
cos(the) * cos(phi),
sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi),
cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)
],
[
cos(the) * sin(phi),
sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi),
cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi)
], [-sin(the),
sin(psi) * cos(the),
cos(psi) * cos(the)]])
# Manual Construction
# Q3 = [cos(psi) -sin(psi) 0;sin(psi) cos(psi) 0;0 0 1]; %Rotation mappings
# Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)];
# Q1 = [1 0 0;0 cos(phi) -sin(phi);0 sin(phi) cos(phi)];
# R = Q3*Q2*Q1; %Rotation matrix
# CALCULATE FLYER TIP POSITONS USING COORDINATE FRAME ROTATION
F = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
# Draw flyer rotors
theta = np.linspace(0, 2 * pi, 20)
circle = np.zeros((3, 20))
for j, t in enumerate(theta):
circle[:, j] = np.r_[r * sin(t), r * cos(t), 0]
hub = np.zeros((3, self.nrotors))
tippath = np.zeros((3, 20, self.nrotors))
for i in range(0, self.nrotors):
# points in the inertial frame
hub[:, i] = F @ (z + R @ self.D[:, i])
# Flapping angle scaling for output display - makes it easier to see what flapping is occurring
q = self.flapscale
# Rotor -> Plot frame
Rr = np.array([[
cos(q * a1s[i]),
sin(q * b1s[i]) * sin(q * a1s[i]),
cos(q * b1s[i]) * sin(q * a1s[i])
], [0, cos(q * b1s[i]), -sin(q * b1s[i])],
[
-sin(q * a1s[i]),
sin(q * b1s[i]) * cos(q * a1s[i]),
cos(q * b1s[i]) * cos(q * a1s[i])
]])
tippath[:, :, i] = F @ R @ Rr @ circle
plot3(self.disk[i], hub[0, i] + tippath[0, :, i],
hub[1, i] + tippath[1, :, i], hub[2, i] + tippath[2, :, i])
# Draw flyer
hub0 = F @ z # centre of vehicle
for i in range(0, self.nrotors):
# line from hub to centre plot3([hub(1,N) hub(1,S)],[hub(2,N) hub(2,S)],[hub(3,N) hub(3,S)],'-b')
plot3(self.arm[i], [hub[0, i], hub0[0]], [hub[1, i], hub0[1]],
[hub[2, i], hub0[2]])
# plot a circle at the hub itself
# plot3([hub(1,i)],[hub(2,i)],[hub(3,i)],'o')
# plot the vehicle's centroid on the ground plane
plot3(self.shadow, [z[0], 0], [-z[1], 0], [0, 0])
plot3(self.groundmark, z[0], -z[1], 0)
def __init__(self,
*inputs,
path=True,
pathstyle=None,
shape='triangle',
color="blue",
fill="white",
size=1,
scale='auto',
labels=['X', 'Y'],
square=True,
init=None,
**kwargs):
"""
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param path: plot path taken by vehicle, defaults to True
:type path: bool, optional
:param pathstyle: linestyle for path, defaults to None
:type pathstyle: str or dict, optional
:param shape: vehicle shape: 'triangle' (default) or 'box'
:type shape: str, optional
:param color: vehicle outline color, defaults to "blue"
:type color: str, optional
:param fill: vehicle fill color, defaults to "white"
:type fill: str, optional
:param size: length of vehicle, defaults to 1
:type size: float, optional
:param scale: x- and y-axis scale, defaults to 'auto'
:type scale: 2- or 4-element sequence
:param labels: axis labels (xlabel, ylabel)
:type labels: 2-element tuple 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 ``**kwargs``: common Block options
:return: A VEHICLEPLOT block
:rtype: VehiclePlot instance
Create a vehicle animation similar to the figure below.
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 True.
- Two shapes of vehicle can be drawn, a narrow triangle and a box
(as seen below).
.. 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__(nin=3, inputs=inputs, **kwargs)
self.xdata = []
self.ydata = []
self.type = 'vehicle'
if init is not None:
assert callable(init), 'graphics init function must be callable'
self.init = init
self.square = square
self.path = path
if path:
self.pathstyle = pathstyle
self.color = color
self.fill = fill
if scale != 'auto':
if len(scale) == 2:
scale = scale * 2
self.scale = scale
self.labels = labels
d = size
if shape == 'triangle':
L = d
W = 0.6 * d
vertices = [(L, 0), (-L, -W), (-L, W)]
elif shape == 'box':
L1 = d
L2 = d
W = 0.6 * d
vertices = [(-L1, W), (0.6 * L2, W), (L2, 0.5 * W), (L2, -0.5 * W),
(0.6 * L2, -W), (-L1, -W)]
else:
raise ValueError('bad vehicle shape specified')
self.vertices_hom = sm.e2h(np.array(vertices).T)
self.vertices = np.array(vertices)
def __init__(self,
*inputs,
nin=None,
styles=None,
scale='auto',
labels=None,
grid=True,
tuner: TcpClientTuner = None,
**kwargs):
"""
Create a block that plots input ports against time.
:param nin: number of inputs, defaults to length of style vector if given,
otherwise 1
:type nin: int, optional
:param styles: styles for each line to be plotted
:type styles: optional str or dict, list of strings or dicts; one per line
:param scale: y-axis scale, defaults to 'auto'
:type scale: 2-element sequence
:param labels: vertical axis labels
:type labels: sequence of strings
:param grid: draw a grid, default is on. Can be boolean or a tuple of
options for grid()
:type grid: bool or sequence
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param ``**kwargs``: common Block options
:return: A SCOPE block
:rtype: Scope instance
Create a block that plots input ports against time.
Each line can have its own color or style which is specified by:
- a dict of options for `Line2D <https://matplotlib.org/3.2.2/api/_as_gen/matplotlib.lines.Line2D.html#matplotlib.lines.Line2D>`_ or
- a MATLAB-style linestyle like 'k--'
If multiple lines are plotted then a heterogeneous list of styles, dicts or strings,
one per line must be given.
The vertical scale factor defaults to auto-scaling but can be fixed by
providing a 2-tuple [ymin, ymax]. All lines are plotted against the
same vertical scale.
Examples::
SCOPE()
SCOPE(nin=2)
SCOPE(nin=2, scale=[-1,2])
SCOPE(style=['k', 'r--'])
SCOPE(style='k--')
SCOPE(style={'color:', 'red, 'linestyle': '--''})
.. figure:: ../../figs/Figure_1.png
:width: 500px
:alt: example of generated graphic
Example of scope display.
"""
self.type = 'scope'
if styles is not None:
self.styles = list(styles)
if nin is not None:
assert nin == len(styles), 'need one style per input'
else:
nin = len(styles)
if labels is not None:
self.labels = list(labels)
if nin is not None:
assert nin == len(labels), 'need one label per input'
else:
nin = len(labels)
else:
self.labels = labels
if nin is None:
nin = 1
super().__init__(nin=nin, inputs=inputs, **kwargs)
if styles is None:
self.styles = [None] * nin
self.grid = grid
# init the arrays that hold the data
self.tdata = np.array([])
self.ydata = [
np.array([]),
] * nin
self.line = [None] * nin
self.scale = scale
self.labels = labels
self.tuner = tuner
self.scope_id = tuner.register_signal_scope(self.name, nin, styles=styles, labels=labels) \
if tuner else None
