def start(self):
quad = self.model
# vehicle dimensons
d = quad['d'] # Hub displacement from COG
r = quad['r'] # Rotor radius
# C = np.zeros((3, self.nrotors)) ## WHERE USED?
self.D = np.zeros((3, self.nrotors))
for i in range(0, self.nrotors):
theta = i / self.nrotors * 2 * pi
# Di Rotor hub displacements (1x3)
# first rotor is on the x-axis, clockwise order looking down from above
self.D[:, i] = np.r_[quad['d'] * cos(theta),
quad['d'] * sin(theta), quad['h']]
# draw ground
self.fig = plt.figure()
# no axes in the figure, create a 3D axes
self.ax = self.fig.add_subplot(111,
projection='3d',
proj_type=self.projection)
# ax.set_aspect('equal')
self.ax.set_xlabel('X')
self.ax.set_ylabel('Y')
self.ax.set_zlabel('-Z (height above ground)')
# TODO allow user to set maximum height of plot volume
self.ax.set_xlim(self.scale[0], self.scale[1])
self.ax.set_ylim(self.scale[2], self.scale[3])
self.ax.set_zlim(0, self.scale[4])
# plot the ground boundaries and the big cross
self.ax.plot([self.scale[0], self.scale[2]],
[self.scale[1], self.scale[3]], [0, 0], 'b-')
self.ax.plot([self.scale[0], self.scale[3]],
[self.scale[1], self.scale[2]], [0, 0], 'b-')
self.ax.grid(True)
self.shadow, = self.ax.plot([0, 0], [0, 0], 'k--')
self.groundmark, = self.ax.plot([0], [0], [0], 'kx')
self.arm = []
self.disk = []
for i in range(0, self.nrotors):
h, = self.ax.plot([0], [0], [0])
self.arm.append(h)
if i == 0:
color = 'b-'
else:
color = 'g-'
h, = self.ax.plot([0], [0], [0], color)
self.disk.append(h)
self.a1s = np.zeros((self.nrotors, ))
self.b1s = np.zeros((self.nrotors, ))
def __init__(self, *inputs, x0=None, L=1, vlim=1, slim=1, **kwargs):
"""
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param x0: Inital state, defaults to 0
:type x0: array_like, optional
:param L: Wheelbase, defaults to 1
:type L: float, optional
:param vlim: Velocity limit, defaults to 1
:type vlim: float, optional
:param slim: Steering limit, defaults to 1
:type slim: float, optional
:param ``**kwargs``: common Block options
:return: a BICYCLE block
:rtype: Bicycle instance
Create a vehicle model with Bicycle kinematics.
Bicycle kinematic model with state :math:`[x, y, \theta]`.
The block has two input ports:
1. Vehicle speed (metres/sec). The velocity limit ``vlim`` is
applied to the magnitude of this input.
2. Steering wheel angle (radians). The steering limit ``slim``
is applied to the magnitude of this input.
and three output ports:
1. x position in the world frame (metres)
2. y positon in the world frame (metres)
3. heading angle with respect to the world frame (radians)
"""
super().__init__(nin=2, nout=3, inputs=inputs, **kwargs)
self.nstates = 3
self.vlim = vlim
self.slim = slim
self.type = 'bicycle'
self.L = L
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(('x', 'y', r'$\theta$'))
self.state_names(('x', 'y', r'$\theta$'))
def __init__(self, *inputs, R=1, W=1, x0=None, **kwargs):
r"""
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param x0: Inital state, defaults to 0
:type x0: array_like, optional
:param R: Wheel radius, defaults to 1
:type R: float, optional
:param W: Wheel separation in lateral direction, defaults to 1
:type R: float, optional
:param ``**kwargs``: common Block options
:return: a DIFFSTEER block
:rtype: DifSteer instance
Create a differential steer vehicle model with Unicycle kinematics, with inputs
given as wheel angular velocity.
Unicycle kinematic model with state :math:`[x, y, \theta]`.
The block has two input ports:
1. Left-wheel angular velocity (radians/sec).
2. Right-wheel angular velocity (radians/sec).
and three output ports:
1. x position in the world frame (metres)
2. y positon in the world frame (metres)
3. heading angle with respect to the world frame (radians)
Note:
- wheel velocity is defined such that if both are positive the vehicle
moves forward.
"""
super().__init__(nin=2, nout=3, inputs=inputs, **kwargs)
self.nstates = 3
self.type = 'diffsteer'
self.R = R
self.W = W
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
def __init__(self, *inputs, x0=None, **kwargs):
r"""
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param x0: Inital state, defaults to 0
:type x0: array_like, optional
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param ``**kwargs``: common Block options
:return: a UNICYCLE block
:rtype: Unicycle instance
Create a vehicle model with Unicycle kinematics.
Unicycle kinematic model with state :math:`[x, y, \theta]`.
The block has two input ports:
1. Vehicle speed (metres/sec).
2. Angular velocity (radians/sec).
and three output ports:
1. x position in the world frame (metres)
2. y positon in the world frame (metres)
3. heading angle with respect to the world frame (radians)
"""
super().__init__(nin=2, nout=3, inputs=inputs, **kwargs)
self.nstates = 3
self.type = 'unicycle'
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
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 __init__(self,
*inputs,
A=None,
B=None,
C=None,
x0=None,
verbose=False,
**kwargs):
r"""
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
: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 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 state-space 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 SS constructor')
self.type = 'LTI SS'
assert A.shape[0] == A.shape[1], 'A must be square'
n = A.shape[0]
if len(B.shape) == 1:
nin = 1
B = B.reshape((n, 1))
else:
nin = B.shape[1]
assert B.shape[0] == n, 'B must have same number of rows as A'
if len(C.shape) == 1:
nout = 1
assert C.shape[0] == n, 'C must have same number of columns as A'
C = C.reshape((1, n))
else:
nout = C.shape[0]
assert C.shape[1] == n, 'C must have same number of columns as A'
super().__init__(nin=nin, nout=nout, inputs=inputs, **kwargs)
self.A = A
self.B = B
self.C = C
self.nstates = A.shape[0]
if x0 is None:
self._x0 = np.zeros((self.nstates, ))
else:
self._x0 = x0
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 __init__(self,
model,
*inputs,
groundcheck=True,
speedcheck=True,
x0=None,
**kwargs):
r"""
:param model: Vehicle geometric and inertial parameters
:type model: dict
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param groundcheck: Prevent vehicle moving below ground , defaults to True
:type groundcheck: bool
:param speedcheck: Check for zero rotor speed, defaults to True
:type speedcheck: bool
:param x0: Initial state, defaults to all zeros
:type x0: TYPE, optional
:param ``**kwargs``: common Block options
:return: a MULTIROTOR block
:rtype: MultiRotor instance
Create a a multi-rotor dynamic model block.
The block has one input port which is a vector of input rotor speeds
in (radians/sec). These are, looking down, clockwise from the front rotor
which lies on the x-axis.
The block has one output port which is a dictionary signal with the
following items:
- ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]`
- ``vb`` translational velocity in the world frame (metres/sec)
- ``w`` angular rates in the world frame as yaw-pitch-roll rates (radians/second)
- ``a1s`` longitudinal flapping angles (radians)
- ``b1s`` lateral flapping angles (radians)
Based on MATLAB code developed by Pauline Pounds 2004.
"""
super().__init__(nin=1, nout=1, inputs=inputs, **kwargs)
self.type = 'quadrotor'
try:
nrotors = model['nrotors']
except KeyError:
raise RuntimeError('vehicle model does not contain nrotors')
assert nrotors % 2 == 0, 'Must have an even number of rotors'
self.nstates = 12
if x0 is not None:
assert len(x0) == self.nstates, "x0 is the wrong length"
else:
x0 = np.zeros((self.nstates, ))
self._x0 = x0
self.nrotors = nrotors
self.model = model
self.groundcheck = groundcheck
self.speedcheck = speedcheck
self.D = np.zeros((3, self.nrotors))
for i in range(0, self.nrotors):
theta = i / self.nrotors * 2 * pi
# Di Rotor hub displacements (1x3)
# first rotor is on the x-axis, clockwise order looking down from above
self.D[:, i] = np.r_[model['d'] * cos(theta),
model['d'] * sin(theta), model['h']]
self.a1s = np.zeros((self.nrotors, ))
self.b1s = np.zeros((self.nrotors, ))
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)