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

coords.py

+24 −25
Original line number Diff line number Diff line 
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:



@njit

def body_to_inertial(vector: np.ndarray, dcm: np.ndarray) -> np.ndarray:

    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.')

    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

+20 −5
Original line number Diff line number Diff line 
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

+17 −7
Original line number Diff line number Diff line 
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

+38 −9
Original line number Diff line number Diff line
@@ -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

+30 −2
Original line number Diff line number Diff line
@@ -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