import numpy as np
from math import sin, cos, atan2, tan, sqrt, pi
import matplotlib.pyplot as plt
import time
from bdsim.block_types import GraphicsBlock, ContinuousBlock, FunctionBlock
[docs]
class MultiRotor(ContinuousBlock):
r"""
:blockname:`MULTIROTOR`
Dynamic model of a multi-rotor flying robot.
:inputs: 1
:outputs: 1
:states: 16
.. list-table::
:header-rows: 1
* - Port type
- Port number
- Types
- Description
* - Input
- 0
- ndarray(N)
- :math:`\varpi`, rotor velocities in (radians/sec)
* - Output
- 0
- dict
- :math:`\mathit{x}`, vehicle state
Dynamic model of a multi-rotor flying robot that includes rotor flapping. The
vehicle state is a dict containing the following items:
- ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]`
- ``trans`` position and velocity in the world frame as
:math:`[x, y, z, \dot{x}, \dot{y}, \dot{z}]`
- ``rot`` orientation and angular rate in the world frame as
:math:`[\theta_Y, \theta_P, \theta_R, \dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
- ``vb`` translational velocity in the body frame as
:math:`[\dot{x}, \dot{y}, \dot{z}]`
- ``w`` angular rates in the body frame as
:math:`[\dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
- ``a1s`` longitudinal flapping angles (radians)
- ``b1s`` lateral flapping angles (radians)
- ``X`` full state vector as
:math:`[x, y, z, \theta_Y, \theta_P, \theta_R, \dot{x}, \dot{y}, \dot{z}, \dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
The dynamic model is a dict with the following key/value pairs.
=========== ==========================================
key description
=========== ==========================================
``nrotors`` Number of rotors (even integer)
``J`` Flyer rotational inertia matrix (3x3)
``h`` Height of rotors above CoG
``d`` Length of flyer arms
``nb`` Number of blades per rotor
``r`` Rotor radius
``c`` Blade chord
``e`` Flapping hinge offset
``Mb`` Rotor blade mass
``Mc`` Estimated hub clamp mass
``ec`` Blade root clamp displacement
``Ib`` Rotor blade rotational inertia
``Ic`` Estimated root clamp inertia
``mb`` Static blade moment
``Ir`` Total rotor inertia
``Ct`` Non-dim. thrust coefficient
``Cq`` Non-dim. torque coefficient
``sigma`` Rotor solidity ratio
``thetat`` Blade tip angle
``theta0`` Blade root angle
``theta1`` Blade twist angle
``theta75`` 3/4 blade angle
``thetai`` Blade ideal root approximation
``a`` Lift slope gradient
``A`` Rotor disc area
``gamma`` Lock number
=========== ==========================================
.. note::
- Based on MATLAB code developed by Pauline Pounds 2004.
- SI units are used.
- Rotor velocity is defined looking down, clockwise from the front rotor
which lies on the x-axis.
:References:
- Design, Construction and Control of a Large Quadrotor micro air vehicle.
P.Pounds, `PhD thesis <https://openresearch-repository.anu.edu.au/handle/1885/146543>`_
Australian National University, 2007.
- Robotics, Vision & Control by Peter Corke, sec 4.2 in all editions
:seealso: :class:`MultiRotorMixer` :class:`MultiRotorPlot`
"""
nin = 1
nout = 1
# Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04
# A simulation of idealised X-4 Flyer II flight dynamics.
# version 2.0 2005 modified to be compatible with latest version of Matlab
# version 3.0 2006 fixed rotation matrix problem
# version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring
# version 5.0 8/8/11, simplified and restructured
# version 6.0 25/10/13, fixed rotation matrix/inverse wronskian definitions, flapping cross-product bug
#
# New in version 2:
# - Generalised rotor thrust model
# - Rotor flapping model
# - Frame aerodynamic drag model
# - Frame aerodynamic surfaces model
# - Internal motor model
# - Much coolage
#
# Version 1.3
# - Rigid body dynamic model
# - Rotor gyroscopic model
# - External motor model
#
# ARGUMENTS
# u Reference inputs 1x4
# tele Enable telemetry (1 or 0) 1x1
# crash Enable crash detection (1 or 0) 1x1
# init Initial conditions 1x12
#
# INPUTS
# u = [N S E W]
# NSEW motor commands 1x4
#
# CONTINUOUS STATES
# z Position 3x1 (x,y,z)
# v Velocity 3x1 (xd,yd,zd)
# n Attitude 3x1 (Y,P,R)
# o Angular velocity 3x1 (wx,wy,wz)
# w Rotor angular velocity 4x1
#
# Notes: z-axis downward so altitude is -z(3)
#
# CONTINUOUS STATE MATRIX MAPPING
# x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4]
#
#
# CONTINUOUS STATE EQUATIONS
# z` = v
# v` = g*e3 - (1/m)*T*R*e3
# I*o` = -o X I*o + G + torq
# R = f(n)
# n` = inv(W)*o
#
[docs]
def __init__(self, model, groundcheck=True, speedcheck=True, x0=None, **blockargs):
r"""
Create a multi-rotor dynamic model block.
:param model: A dictionary of vehicle geometric and inertial properties
:type model: dict
:param groundcheck: Prevent vehicle moving below ground :math:`z>0`, defaults to True
:type groundcheck: bool
:param speedcheck: Check for non-positive rotor speed, defaults to True
:type speedcheck: bool
:param x0: Initial state, defaults to None
:type x0: array_like(6) or array_like(12), optional
:param blockargs: |BlockOptions|
:type blockargs: dict
"""
if model is None:
raise ValueError("no model provided")
super().__init__(nin=1, nout=1, **blockargs)
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 None:
x0 = np.zeros((self.nstates,))
else:
x0 = np.r_[x0]
if len(x0) == 6:
# assume all derivative are zero
x0 = np.r_[x0, np.zeros((6,))]
elif len(x0) == 4:
# assume x,y,z,yaw
x0 = np.r_[x0[:3], 0, 0, x0[3], np.zeros((6,))]
elif len(x0) == 3:
# assume x,y,z
x0 = np.r_[x0[:3], np.zeros((9,))]
elif len(x0) != self.nstates:
raise ValueError("x0 is the wrong length")
self._x0 = x0
self.nrotors = nrotors
self.model = model
self.groundcheck = groundcheck
self.speedcheck = speedcheck
self.D = np.zeros((3, self.nrotors))
self.theta = np.zeros((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.theta[i] = theta
self.a1s = np.zeros((self.nrotors,))
self.b1s = np.zeros((self.nrotors,))
def output(self, t, inports, x):
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 = 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
vd = np.linalg.inv(R) @ x[6:9] # translational velocity mapped to body frame
rpyd = iW @ x[9:12] # RPY rates mapped to body frame
out = {}
out["x"] = x[0:6]
out["trans"] = np.r_[x[:3], vd]
out["rot"] = np.r_[x[3:6], rpyd]
out["vb"] = (
np.linalg.inv(R) @ x[6:9]
) # translational velocity mapped to body frame
out["w"] = iW @ x[9:12] # RPY rates mapped to body frame
out["a1s"] = self.a1s
out["b1s"] = self.b1s
out["X"] = np.r_[x[:6], vd, rpyd]
# sys = [ x(1:6);
# inv(R)*x(7:9); % translational velocity mapped to body frame
# iW*x(10:12)];
return [out]
def deriv(self, t, inports, x):
model = self.model
# Body-fixed frame references
# ei Body fixed frame references 3x1
e3 = np.r_[0, 0, 1]
# process inputs
w = inports[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 = x[0:3] # position in {W}
n = x[3:6] # RPY angles {W}
v = x[6:9] # velocity in {W}
o = x[9:12] # angular velocity in {W}
# PREPROCESS ROTATION AND WRONSKIAN MATRICIES
phi = n[0] # yaw
the = n[1] # pitch
psi = n[2] # roll
# phi = n(1); % yaw
# the = n(2); % pitch
# psi = n(3); % 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)
# % rotz(phi)*roty(the)*rotx(psi)
# R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); %BBF > Inertial rotation matrix
# 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)];
# iW = [0 sin(psi) cos(psi); %inverted Wronskian
# 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
mu = sqrt(np.sum(Vr[0:2] ** 2)) / (
abs(w[i]) * model["r"]
) # Magnitude of mu, planar components
lc = Vr[2] / (abs(w[i]) * model["r"]) # Non-dimensionalised normal inflow
li = mu # Non-dimensionalised induced velocity approximation
alphas = atan2(lc, mu)
j = atan2(Vr[1], Vr[0]) # Sideslip azimuth relative to e1 (zero over nose)
J = np.array(
[[cos(j), -sin(j)], [sin(j), cos(j)]]
) # BBF > mu sideslip rotation matrix
# Flapping (2,) vector of longitudinal and lateral flapping angles in the rotor plane
beta = np.array(
[
((8 / 3 * model["theta0"] + 2 * model["theta1"]) * mu - 2 * lc * mu)
/ (1 - mu**2 / 2), # Longitudinal flapping
0, # Lattitudinal flapping (note sign)
]
)
# sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)];
beta = J.T @ beta
# Rotate the beta flapping angles to longitudinal and lateral coordinates.
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
)
tau[:, i] = np.cross(T[:, i], self.D[:, i]) # Torque due to rotor thrust
# print(f"{tau=}")
# print(f"{T=}")
# RIGID BODY DYNAMIC MODEL
dz = v
dn = iW @ o
dv = model["g"] * e3 + R @ np.sum(T, axis=1) / model["M"]
do = -np.linalg.inv(model["J"]) @ (
np.cross(o, model["J"] @ o) + np.sum(tau, axis=1) + np.sum(Q, axis=1)
) # row sum of torques
# dv = quad.g*e3 + R*(1/quad.M)*sum(T,2);
# do = inv(quad.J)*(cross(-o,quad.J*o) + sum(tau,2) + sum(Q,2)); %row sum of torques
# vehicle can't fall below ground, remember z is down
if self.groundcheck and z[2] > 0:
z[0] = 0
dz[0] = 0
# # 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
# ------------------------------------------------------------------------ #
[docs]
class MultiRotorMixer(FunctionBlock):
r"""
:blockname:`MULTIROTORMIXER`
Speed mixer for a multi-rotor flying vehicle.
:inputs: 4
:outputs: 1
:states: 0
.. list-table::
:header-rows: 1
* - Port type
- Port number
- Types
- Description
* - Input
- 0
- float
- :math:`\tau_R`, roll torque
* - Input
- 1
- float
- :math:`\tau_P`, pitch torque
* - Input
- 2
- float
- :math:`\tau_Y`, yaw torque
* - Input
- 3
- float
- :math:`T`, total thrust
* - Output
- 0
- ndarray(N)
- :math:`\varpi`, rotor speeds
This block converts airframe moments and total thrust into a 1D
array of rotor speeds which can be input to the ``MULTIROTOR`` block.
The model is a dict with the following key/value pairs.
=========== ==========================================
key description
=========== ==========================================
``nrotors`` Number of rotors (even integer)
``h`` Height of rotors above CoG
``d`` Length of flyer arms
``r`` Rotor radius
=========== ==========================================
.. note:: Based on MATLAB code developed by Pauline Pounds 2004.
:seealso: :class:`MultiRotor` :class:`MultiRotorPlot`
"""
nin = 4
nout = 1
inlabels = ("τr", "τp", "τy", "T")
outlabels = ("ω",)
[docs]
def __init__(self, model=None, wmax=1000, wmin=5, **blockargs):
"""
:param model: A dictionary of vehicle geometric and inertial properties
:type model: dict
:param maxw: maximum rotor speed in rad/s, defaults to 1000
:type maxw: float
:param minw: minimum rotor speed in rad/s, defaults to 5
:type minw: float
:param blockargs: |BlockOptions|
:type blockargs: dict
"""
if model is None:
raise ValueError("no model provided")
super().__init__(**blockargs)
self.type = "multirotormixer"
self.model = model
self.nrotors = model["nrotors"]
self.minw = wmin**2
self.maxw = wmax**2
self.theta = np.arange(self.nrotors) / self.nrotors * 2 * np.pi
# build the Nx4 mixer matrix
M = []
s = []
for i in range(self.nrotors):
# roll and pitch coupling
column = np.r_[
-sin(self.theta[i]) * model["d"] * model["b"],
cos(self.theta[i]) * model["d"] * model["b"],
model["k"] if (i % 2) == 0 else -model["k"],
-model["b"],
]
s.append(1 if (i % 2) == 0 else -1)
M.append(column)
self.M = np.array(M).T
self.Minv = np.linalg.inv(self.M)
self.signs = np.array(s)
def output(self, t, inports, x):
tau = inports
# mix airframe force/torque to rotor thrusts
w = self.Minv @ tau
# clip the rotor speeds to the range [minw, maxw]
w = np.clip(w, self.minw, self.maxw)
# convert required thrust to rotor speed
w = np.sqrt(w)
# flip the signs of alternating rotors
w = self.signs * w
return [w]
# ------------------------------------------------------------------------ #
[docs]
class MultiRotorPlot(GraphicsBlock):
r"""
:blockname:`MULTIROTORPLOT`
Displays/animates a multi-rotor flying vehicle.
:inputs: 1
:outputs: 0
:states: 0
.. list-table::
:header-rows: 1
* - Port type
- Port number
- Types
- Description
* - Input
- 0
- dict
- :math:`\mathit{x}`, vehicle state
Animate a multi-rotor flying vehicle using Matplotlib graphics. The
rotors are shown as circles and their orientation includes rotor
flapping which can be exagerated by ``flapscale``.
.. figure:: ../figs/multirotorplot.png
:width: 500px
:alt: example of generated graphic
Example of quad-rotor display.
The input is a dictionary signal and the block requires the items:
- ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]`
- ``a1s`` rotor flap angle
- ``b1s`` rotor flap angle
The model is a dict with the following key/value pairs.
=========== ==========================================
key description
=========== ==========================================
``nrotors`` Number of rotors (even integer)
``h`` Height of rotors above CoG
``d`` Length of flyer arms
``r`` Rotor radius
=========== ==========================================
.. note:: Based on MATLAB code developed by Pauline Pounds 2004.
:seealso: :class:`MultiRotor` :class:`MultiRotorMixer`
"""
nin = 1
nout = 0
inlabels = ("x",)
PLOT3D = True
TIMESTAMP = True
# Based on code lovingly coded by Paul Pounds, first coded 17/4/02
# version 2 2004 added scaling and ground display
# version 3 2010 improved rotor rendering and fixed mirroring bug
# Displays X-4 flyer position and attitude in a 3D plot.
# GREEN ROTOR POINTS NORTH
# BLUE ROTOR POINTS EAST
# PARAMETERS
# s defines the plot size in meters
# swi controls flyer attitude plot; 1 = on, otherwise off.
# INPUTS
# 1 Center X position
# 2 Center Y position
# 3 Center Z position
# 4 Yaw angle in rad
# 5 Pitch angle in rad
# 6 Roll angle in rad
[docs]
def __init__(
self,
model,
scale=None,
flapscale=1,
projection="ortho",
**blockargs,
):
"""
:param model: A dictionary of vehicle geometric and inertial properties
:type model: dict
:param scale: dimensions of workspace: xmin, xmax, ymin, ymax, zmin, zmax, defaults to [-2,2,-2,2,10]
:type scale: array_like, optional
:param flapscale: exagerate flapping angle by this factor, defaults to 1
:type flapscale: float
:param projection: 3D projection, one of: 'ortho' [default], 'perspective'
:type projection: str
:param blockargs: |GraphicsBlockOptions|
:type blockargs: dict
"""
if model is None:
raise ValueError("no model provided")
if scale is None:
scale = (-2, 2, -2, 2, 10)
else:
scale = tuple(scale)
super().__init__(nin=1, **blockargs)
self.type = "quadrotorplot"
self.model = model
self.scale = scale
self.nrotors = model["nrotors"]
self.projection = projection
self.flapscale = flapscale
def start(self, simstate):
super().start(simstate)
if not self._enabled:
return
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
assert self.fig is not None
assert self.ax is not None
self.ax.set_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)")
self.panel = self.ax.text2D(
0.05,
0.95,
"",
transform=self.ax.transAxes,
fontsize=10,
family="monospace",
verticalalignment="top",
bbox=dict(boxstyle="round", facecolor="white", edgecolor="black"),
)
# 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[1]], [self.scale[2], self.scale[3]], [0, 0], "b-"
)
self.ax.plot(
[self.scale[0], self.scale[1]], [self.scale[3], 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,))
# plt.draw()
# plt.show(block=False)
def step(self, t, inports):
if not self._enabled:
return
def plot3(h, x, y, z):
h.set_data_3d(x, y, z)
# h.set_data(x, y)
# h.set_3d_properties(np.r_[z])
# read UAV output "bus"
input = inports[0]
z = input["x"][0:3]
n = input["x"][3:6]
# TODO, check input dimensions, 12 or 12+2N, deal with flapping
a1s = input["a1s"]
b1s = input["b1s"]
quad = self.model
# vehicle dimensons
d = quad["d"] # Hub displacement from COG
r = quad["r"] # Rotor radius
# PREPROCESS ROTATION MATRIX
phi, the, psi = n # Euler angles
# 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):
hub[:, i] = F @ (z + R @ self.D[:, i]) # points in the inertial frame
q = (
self.flapscale
) # Flapping angle scaling for output display - makes it easier to see what flapping is occurring
# 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])
textstr = f"t={t: .2f}\nh={z[2]: .2f}\nγ={n[0]: .2f}"
self.panel.set_text(textstr)
super().step(t, inports)
if __name__ == "__main__":
from quad_model import quadrotor
# m = MultiRotorMixer(model=quadrotor)
# print(m.M)
# print(m.Minv)
# # print(m.Minv @ [0, 0, 0, -40])
# m.inputs = [0, 0, 0, -40]
# print(m.output(0.0))
# m.inputs = [0, 0, 0, -50]
# print(m.output(0.0))
# m.inputs = [0, 0, 0.1, -40]
# print(m.output(0.0))
# m.inputs = [1, 0, 0, -40]
# print(m.output(0.0))
# m.inputs = [0, 1, 0, -40]
# print(m.output(0.0))
m = MultiRotor(model=quadrotor)
def show(w):
print()
print(w[0] ** 2 + w[2] ** 2 - w[1] ** 2 - w[3] ** 2)
print(w)
x = np.r_[0.0, 0, -4, 0, 0, 0, 0, 0, 0, 0, 0, 0]
inputs = [np.r_[w]]
dx = m.deriv(0.0, inputs, x)
print("zdd", dx[8])
print("wd", dx[9:12])
x = dx
x = m.output(0.0, inputs, x)[0]["X"]
print("zd", x[8])
print("ypr_dot", x[9:12])
show([800.0, -800, 800, -800])
# tau_y pitch
z = np.sqrt((900**2 + 700**2) / 2)
show([900.0, -z, 700, -z])
show([700.0, -z, 900, -z])
# tau_x roll
show([z, -900, z, -700])
show([z, -700, z, -900])
# tau_z roll
show([900, -800, 900, -800])