Visualization in jupyter notebook;

Trajectory of multirotor shows only after the loop with all actions is finished. Need to find way to make updates to plot live.

Errors when plotting outside notebook environment via ssh with x-display. TODO.

coords.py

 import numpy as np
 from numpy import sin, cos, tan
+from numba import njit
 
 
-def body_to_inertial(coords: np.ndarray, *, rotations=None, dcm=None, dcm_inverse=None) -> np.ndarray:
-    if rotations is not None:
-        dcm = direction_cosine_matrix(yaw=rotations[0], pitch=rotations[1], roll=rotations[2])
-    if dcm is not None:
-        dcm_inverse = dcm.T
-    if dcm_inverse is not None:
-        return dcm_inverse @ coords
-    else:
-        raise ValueError('Specify one of yaw, pitch, roll angles, direction cosine matrix, or its inverse.')
 
+@njit
+def body_to_inertial(vector: np.ndarray, dcm: np.ndarray) -> np.ndarray:
+    dcm_inverse = dcm.T
+    return dcm_inverse @ vector
 
 
-def inertial_to_body(coords: np.ndarray, *, rotations=None, dcm=None) -> np.ndarray:
-    if rotations is not None:
-        dcm = direction_cosine_matrix(yaw=rotations[0], pitch=rotations[1], roll=rotations[2])
-    if dcm is not None:
-        return dcm @ coords
-    else:
-        raise ValueError('Specify one of yaw, pitch, roll angles or direction cosine matrix.')
 
+@njit
+def inertial_to_body(vector: np.ndarray, dcm=np.ndarray) -> np.ndarray:
+    return dcm @ vector
 
 
-def direction_cosine_matrix(yaw: float, pitch: float, roll: float) -> np.matrix:
-    cy = cos(yaw)
-    sy = sin(yaw)
-    cp = cos(pitch)
-    sp = sin(pitch)
+
+@njit
+def direction_cosine_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray:
     cr = cos(roll)
     sr = sin(roll)
+    cp = cos(pitch)
+    sp = sin(pitch)
+    cy = cos(yaw)
+    sy = sin(yaw)
     dcm = np.asarray([
         [cp * cy,                   cp * sy,                    -sp],
         [-cr * sy + sr * sp * cy,   cr * cy + sr * sp * sy,     sr * cp],
         [sr * sy + cr * sp * cy,    -sr * cy + cr * sp * sy,    cr * cp]
     ])
-    return np.asmatrix(dcm)
+    return dcm
 
 
 
+@njit
 def rotating_frame_derivative(value: np.ndarray, local_derivative: np.ndarray, omega: np.ndarray) -> np.ndarray:
     # d (value . vector) / dt
     # = vector . d value / dt + value . d vector / dt
@@ -53,6 +48,7 @@ def rotating_frame_derivative(value: np.ndarray, local_derivative: np.ndarray, o
 
 
 
+@njit
 def angular_to_euler_rate(angular_velocity: np.ndarray, orientation: np.ndarray) -> np.ndarray:
     roll, pitch, yaw = orientation
     p, q, r = angular_velocity
@@ -63,10 +59,13 @@ def angular_to_euler_rate(angular_velocity: np.ndarray, orientation: np.ndarray)
 
 
 
+@njit
 def euler_to_angular_rate(euler_velocity: np.ndarray, orientation: np.ndarray) -> np.ndarray:
     roll_rate, pitch_rate, yaw_rate = euler_velocity
     roll, pitch, yaw = orientation
-    p = roll_rate - yaw_rate * sin(pitch)
-    q = pitch_rate * cos(roll) + yaw_rate * cos(pitch) * sin(roll)
-    r = yaw_rate * cos(roll) * cos(pitch) - pitch_rate * sin(roll)
+    cr, cp = cos(roll), cos(pitch)
+    sr, sp = sin(roll), sin(pitch)
+    p = roll_rate - yaw_rate * sp
+    q = pitch_rate * cr + yaw_rate * cp * sr
+    r = yaw_rate * cr * cp - pitch_rate * sr
     return np.asarray([p, q, r])
\ No newline at end of file

helpers.py

-from typing import Iterable
+from typing import Callable, Iterable
 
 import numpy as np
+from scipy.optimize import fsolve
 
 from vehicle import PropellerParams, VehicleParams
 from simulation import Propeller
 
 
 
-def moment_of_inertia_tensor_from_cooords(point_masses: Iterable[float], coords: Iterable[np.ndarray]) -> np.matrix:
+def moment_of_inertia_tensor_from_cooords(
+    point_masses: Iterable[float], coords: Iterable[np.ndarray]
+) -> np.matrix:
     coords = np.asarray(coords)
     masses = np.asarray(point_masses)
     x,y,z = coords[:,0], coords[:,1], coords[:2]
@@ -25,9 +28,12 @@ def moment_of_inertia_tensor_from_cooords(point_masses: Iterable[float], coords:
 
 
 
-def vehicle_params_factory(n: int, m_prop: float, d_prop: float, params: PropellerParams,
-    m_body: float, body_shape: str='point', body_size: float=0.1):
-    angles = np.linspace(0, 2 * np.pi, num=n, endpoint=False)
+def vehicle_params_factory(
+    n: int, m_prop: float, d_prop: float, params: PropellerParams,
+    m_body: float, body_shape: str='sphere_solid', body_size: float=0.1
+):
+    angle_spacing = 2 * np.pi / n
+    angles = np.arange(angle_spacing / 2, 2 * np.pi, angle_spacing)
     masses = [m_prop] * n
     x = np.cos(angles) * d_prop
     y = np.sin(angles) * d_prop
@@ -53,3 +59,12 @@ def vehicle_params_factory(n: int, m_prop: float, d_prop: float, params: Propell
         mass = n * m_prop + m_body,
         inertia_matrix = I
     )
+
+
+
+def find_nominal_speed(thrust_fn: Callable[[float], float], weight: float) -> float:
+    def balance(speed: float) -> float:
+        thrust = thrust_fn(speed)
+        residual = thrust - weight
+        return residual
+    return fsolve(balance, 1e3)[0]
\ No newline at end of file

physics.py

-from typing import Iterable
+from typing import Iterable, Tuple
 
 import numpy as np
 from numba import njit, jit
@@ -8,8 +8,6 @@ from scipy.optimize import fsolve
 
 @njit
 def thrustEqn(vi, *prop_params):
-    
-    # Unpack parameters
     R,A,rho,a,b,c,eta,theta0,theta1,u,v,w,Omega = prop_params
     
     # Calculate local airflow velocity at propeller with vi, V'
@@ -26,7 +24,10 @@ def thrustEqn(vi, *prop_params):
 
 
 
-def thrust(speed, airstream_velocity, R, A, rho, a, b, c, eta, theta0, theta1) -> float:
+def thrust(
+    speed, airstream_velocity, R, A, rho, a, b, c, eta, theta0, theta1,
+    vi_guess=0.1
+) -> float:
     u, v, w = airstream_velocity
     # Convert commanded RPM to rad/s
     Omega = 2 * np.pi / 60 * speed
@@ -36,7 +37,7 @@ def thrust(speed, airstream_velocity, R, A, rho, a, b, c, eta, theta0, theta1) -
     
     # Numerically solve for propeller induced velocity, vi
     # using nonlinear root finder, fsolve, and prop_params
-    # TODO: numba jit gives error for this function ('Untyped global name fsolve')
+    # TODO: numba jit gives error for fsolve ('Untyped global name fsolve')
     vi = fsolve(thrustEqn, 0.1, args=prop_params)
     
     # Plug vi back into Thrust equation to solve for T
@@ -54,8 +55,10 @@ def torque(position_vector: np.ndarray, thrust: float) -> np.ndarray:
 
 
 @njit
-def apply_forces_torques(forces: np.ndarray, torques: np.ndarray, x: np.ndarray,
-    g: float, mass: float, inertia_matrix: np.matrix, inertia_matrix_inverse: np.matrix):
+def apply_forces_torques(
+    forces: np.ndarray, torques: np.ndarray, x: np.ndarray, g: float, mass: float,
+    inertia_matrix: np.matrix, inertia_matrix_inverse: np.matrix
+) -> np.ndarray:
     # Store state variables in a readable format
     ub = x[0]
     vb = x[1]
@@ -106,3 +109,10 @@ def apply_forces_torques(forces: np.ndarray, torques: np.ndarray, x: np.ndarray,
     xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot
     
     return xdot
+
+
+
+def control_allocation(allocation_matrix: np.matrix, net_forces: np.array, net_torques: np.array):
+    # [F T] = P . f
+    # P-1 F T = f
+    pass
\ No newline at end of file

simulation.py

@@ -2,11 +2,10 @@ from typing import List, Tuple
 
 import numpy as np
 from numpy import (cos, sin)
-from scipy.optimize import fsolve
 from scipy.integrate import odeint
 
 from vehicle import PropellerParams, SimulationParams, VehicleParams
-from coords import rotating_frame_derivative, angular_to_euler_rate
+from coords import body_to_inertial, rotating_frame_derivative, angular_to_euler_rate
 from physics import thrust, torque, apply_forces_torques
 
 
@@ -18,16 +17,16 @@ class Propeller:
         self.simulation: SimulationParams = simulation
         self.speed: float = None
         "Revolutions per minute"
-        self._last_induced_velocity = 0.1
+        self._induced_velocity_guess = 0.1
         "Guess for the induced velocity about the propeller"
 
 
     def reset(self):
         self.speed = 0.
-        self._last_induced_velocity = 0.1
+        self._induced_velocity_guess = 0.1
 
 
-    def step(self, u: float, dt: float) -> float:
+    def step(self, u: float) -> float:
         return self.apply_speed(u)
 
 
@@ -36,6 +35,15 @@ class Propeller:
         return self.speed
 
 
+    def thrust(self, speed=None, airstream_velocity: np.ndarray=np.zeros(3)) -> float:
+        p = self.params
+        speed = speed or self.speed
+        return thrust(
+            speed, airstream_velocity,
+            p.R, p.A, self.simulation.rho, p.a, p.b, p.c, p.eta, p.theta0, p.theta1,
+            self._induced_velocity_guess)
+
+
     @property
     def state(self) -> float:
         return self.speed
@@ -50,24 +58,28 @@ class Multirotor:
         self.state: np.ndarray = None
         self.propellers: List[Propeller] = None
         self.propeller_vectors: np.matrix = None
+        self.reset()
 
 
     def reset(self):
         self.propellers = []
         for params in self.params.propellers:
             self.propellers.append(Propeller(params, self.simulation))
+
         x = cos(self.params.angles) * self.params.distances
         y = sin(self.params.angles) * self.params.distances
         z = np.zeros_like(y)
         self.propeller_vectors = np.vstack((x, y, z))
+
         self.inertial_matrix_inverse = np.asmatrix(np.linalg.inv(self.params.inertia_matrix))
+
         self.state = np.zeros(12)
 
 
     @property
     def position(self) -> np.ndarray:
         """Navigation coordinates (height = - z coordinate)"""
-        return np.asfarray(self.state[9], self.state[10], -self.state[11])
+        return np.asarray([self.state[9], self.state[10], -self.state[11]])
 
 
     @property
@@ -76,6 +88,13 @@ class Multirotor:
         return self.state[:3]
 
 
+    @property
+    def navigation_velocity(self) -> np.ndarray:
+        v_inertial = body_to_inertial(self.velocity)
+        v_inertial[2] *= -1 # convert inertial to navigation frame (h = -z)
+        return v_inertial
+
+
     @property
     def orientation(self) -> np.ndarray:
         """Euler rotations (roll, pitch, yaw)"""
@@ -101,6 +120,7 @@ class Multirotor:
             self.propeller_vectors,
             linear_vel_body,
             angular_vel_body)
+
         thrust_vec = np.zeros((3, len(self.propellers)))
         torque_vec = np.zeros_like(thrust_vec)
 
@@ -108,7 +128,14 @@ class Multirotor:
                 speeds,
                 self.propellers)):
             speed = prop.apply_speed(speed)
-            thrust_vec[2, i] = -thrust(speed,airstream_velocity_inertial[:, i],prop.params.R, prop.params.A, self.simulation.rho, prop.params.a, prop.params.b, prop.params.c, prop.params.eta, prop.params.theta0, prop.params.theta1)
+            thrust_vec[2, i] = -thrust(
+                        speed, airstream_velocity_inertial[:, i], prop.params.R,
+                        prop.params.A, self.simulation.rho, prop.params.a,
+                        prop.params.b, prop.params.c, prop.params.eta,
+                        prop.params.theta0, prop.params.theta1,
+                        prop._induced_velocity_guess)
+            
+            # Store solution as initial guess for next step's solution
             torque_vec[:, i] = torque(self.propeller_vectors[:,i], -thrust_vec[2,i])
 
         forces = thrust_vec.sum(axis=1)
@@ -118,10 +145,12 @@ class Multirotor:
 
     def dxdt(self, x: np.ndarray, t: float, u: np.ndarray):
         forces, torques = self.get_forces_torques(u)
-        xdot = apply_forces_torques(forces, torques, self.state, self.simulation.g, self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse)
+        xdot = apply_forces_torques(
+            forces, torques, self.state, self.simulation.g,
+            self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse)
         return xdot
 
 
-    def step(self, u: np.ndarray, jit=False):
+    def step(self, u: np.ndarray):
         self.state = odeint(self.dxdt, self.state, (0, self.simulation.dt), args=(u,))[-1]
         return self.state

vehicle.py

@@ -11,7 +11,7 @@ class PropellerParams:
     pitch: float = 3   #inches
 
     a: float = 5.7
-    "Lift curve slope used in example in Stevens & Lewis"
+    "Lift curve slope used in example in Stevens & Lewis" # TODO: read more
     b: float = 2
     "Number of blades"
     c: float = 0.0274
@@ -19,20 +19,44 @@ class PropellerParams:
     eta: float = 1.
     "Propeller efficiency"
 
+    k_lift: float = None
+    "Propeller's aerodynamic lift coefficient"
+    k_drag: float = None
+    "Propeller's aerodynamic drag coefficient"
+
     def __post_init__(self):
-        self.R = self.diameter * 0.0254
+        self.R = self.diameter * 0.0254 # inches to metres
+        "Radius in metres"
         self.A = np.pi * self.R**2
+        "Area of propeller disc in metres squared"
         self.theta0 = 2*np.arctan2(self.pitch, (2 * np.pi * 3/4 * self.diameter/2))
         self.theta1 = -4 / 3 * np.arctan2(self.pitch, 2 * np.pi * 3/4 * self.diameter/2)
 
 
 
+@dataclass
+class MotorParams:
+
+    kt: float
+    "Torque constant"
+    km: float
+    "Motor constant"
+    ke: float = None
+    "Back-EMF constant=Torque constant"
+
+
+    def __post_init__(self):
+        self.ke = self.kt
+
+
+
 @dataclass
 class VehicleParams:
 
     propellers: List[PropellerParams]
     angles: np.ndarray
     distances: np.ndarray
+    clockwise: np.ndarray = None
 
     mass: float = 1.
     inertia_matrix: np.matrix = np.eye(3)
@@ -40,6 +64,10 @@ class VehicleParams:
 
     def __post_init__(self):
         self.inertia_matrix_inverse = np.linalg.inv(self.inertia_matrix)
+        if self.clockwise is None:
+            self.clockwise = np.ones(len(self.propellers), dtype=int)
+            self.clockwise[::2] = 1
+            self.clockwise[1::2] = -1
 
 
 

visualize.py

+from typing import Tuple
+import threading as th
+from dataclasses import dataclass
+import time
+
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from mpl_toolkits.mplot3d.art3d import Line3D
+
+from coords import direction_cosine_matrix, inertial_to_body
+from vehicle import VehicleParams
+from simulation import Multirotor
+
+
+
+@dataclass
+class VehicleDrawing:
+    vehicle: Multirotor
+    axis: Axes3D = None
+    max_frames_per_second: float = 30.
+    trace: bool = False
+
+    def __post_init__(self):
+        self.params = self.vehicle.params
+        self.lines, self.body_frame_segments, self.trajectory_line = make_drawing(self.params)
+        self.interval = 1 / self.max_frames_per_second
+        self.is_ipython = is_ipython()
+        if self.trace:
+            self.trajectory = [[], [], []] # [[X,..], [Y,...], [Z,...]]
+        else:
+            self.ipython_display = None
+
+
+    def connect(self):
+        self.called = 0
+        self.ev_cancel = th.Event()
+        self.thread = th.Thread(target=self._worker, daemon=False)
+        self.thread.start()
+
+
+    def disconnect(self, force=False):
+        self.ev_cancel.set()
+        self.thread.join(timeout=1.5 * self.interval)
+
+
+    def _worker(self):
+        if self.axis is None:
+            self.figure, self.axis = make_fig((-10,10), (-10,10), (0,20))
+        else:
+            self.figure = self.axis.figure
+
+        for l in self.lines:
+            self.axis.add_line(l)
+        self.axis.add_line(self.trajectory_line)
+        
+        # if self.is_ipython:
+        #     from IPython.display import display
+        #     self.ipython_display = display(self.figure, display_id=True)
+        # else:
+        self.ipython_display = None
+        self.figure.show()
+
+        while not self.ev_cancel.is_set():
+            start = time.time()
+            position = self.vehicle.position
+            orientation = self.vehicle.orientation
+            update_drawing(self, position, orientation)
+            end = time.time()
+            self.ev_cancel.wait(self.interval - (end - start))
+        
+
+
+def make_drawing(params: VehicleParams):
+    arms = np.zeros((len(params.propellers) * 2, 3)) # [2 points/ propeller, axis]
+    x = params.distances * np.cos(params.angles)
+    y = params.distances * np.sin(params.angles)
+    arms[1::2,0] = x
+    arms[1::2,1] = y
+    lines = []
+    for i in range(len(params.propellers)):
+        lines.append(
+            Line3D(
+                arms[2*i:2*i+2,0],
+                arms[2*i:2*i+2,1],
+                arms[2*i:2*i+2,2],
+                antialiased=False))
+    trajectory = Line3D([], [], [])
+    return lines, arms, trajectory
+
+
+
+def update_drawing(drawing: VehicleDrawing, position: np.ndarray, orientation: np.ndarray):
+    dcm = direction_cosine_matrix(orientation[0], orientation[1], orientation[2])
+    arms = np.copy(drawing.body_frame_segments)
+    arms = inertial_to_body(arms.T, dcm).T
+    arms += position
+    for i, l in enumerate(drawing.lines):
+        j = 2*i
+        l.set_data(arms[j:j+2, 0], arms[j:j+2, 1])
+        l.set_3d_properties(arms[j:j+2, 2])
+    
+    if drawing.trace:
+        drawing.trajectory[0].append(position[0])
+        drawing.trajectory[1].append(position[1])
+        drawing.trajectory[2].append(position[2])
+        drawing.trajectory_line.set_data(drawing.trajectory[0], drawing.trajectory[1])
+        drawing.trajectory_line.set_3d_properties(drawing.trajectory[2])
+
+    drawing.figure.canvas.draw_idle()
+    drawing.figure.canvas.flush_events()
+    plt.pause(1e-7)
+
+    if drawing.ipython_display is not None:
+        drawing.ipython_display.update(drawing.figure)
+
+
+
+def make_fig(xlim, ylim, zlim) -> Tuple[plt.Figure, Axes3D]:
+    fig = plt.figure()
+    ax = fig.add_subplot(projection='3d', xlim=xlim, ylim=ylim, zlim=zlim)
+    return fig, ax
+
+
+
+def is_ipython() -> bool:
+    # https://stackoverflow.com/q/15411967/4591810
+    # https://stackoverflow.com/a/39662359/4591810
+    try:
+        import IPython
+        shell = IPython.get_ipython().__class__.__name__
+        if ('Terminal' in shell) or ('terminal' in shell) or (shell == 'NoneType'):
+            return False
+        else:
+            return True
+    except NameError:
+        return False
+    except ImportError:
+        return False