Commit e4a103f6 by Ibrahim

### 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 ... ...
visualize.py 0 → 100644
 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!