Commit e4a103f6 authored by Ibrahim's avatar Ibrahim
Browse files

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.
parent 56aaaae2
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
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
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
......@@ -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
......@@ -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
......
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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment