Commit fa3d2d30 by Ibrahim

### torque calculations and convenience functions for testing

parent 0944ee8c
 ... ... @@ -6,8 +6,23 @@ from numba import njit @njit def body_to_inertial(vector: np.ndarray, dcm: np.ndarray) -> np.ndarray: dcm_inverse = dcm.T return dcm_inverse @ vector """ View body coordinates in terms of coordinates on inertial axes. Assumes both frames share same origin. Parameters ---------- vector : np.ndarray The 3D vector of points, or a 3 x N matrix of N points dcm : np.ndarray The 3x3 direction cosine matrix Returns ------- np.ndarray The transformed coordinates in the intertial frame """ return dcm.T @ vector ... ... @@ -19,11 +34,11 @@ def inertial_to_body(vector: np.ndarray, dcm=np.ndarray) -> np.ndarray: @njit def direction_cosine_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray: cr = cos(roll) cr = cos(roll) # phi sr = sin(roll) cp = cos(pitch) cp = cos(pitch) # theta sp = sin(pitch) cy = cos(yaw) cy = cos(yaw) # psi sy = sin(yaw) dcm = np.asarray([ [cp * cy, cp * sy, -sp], ... ... @@ -35,7 +50,9 @@ def direction_cosine_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray @njit def rotating_frame_derivative(value: np.ndarray, local_derivative: np.ndarray, omega: np.ndarray) -> np.ndarray: 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 # ( local derivative ) ( coriolis term ) ... ... @@ -49,7 +66,9 @@ 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: def angular_to_euler_rate( angular_velocity: np.ndarray, orientation: np.ndarray ) -> np.ndarray: roll, pitch, yaw = orientation p, q, r = angular_velocity roll_rate = p + q * tan(pitch) * (q * sin(roll) + r * cos(roll)) ... ... @@ -60,7 +79,9 @@ 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: 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 cr, cp = cos(roll), cos(pitch) ... ...
 from typing import Callable, Iterable from typing import Callable, Iterable, Tuple import numpy as np from scipy.optimize import fsolve ... ... @@ -66,4 +66,20 @@ def find_nominal_speed(thrust_fn: Callable[[float], float], weight: float) -> fl thrust = thrust_fn(speed) residual = thrust - weight return residual return fsolve(balance, 1e3)[0] \ No newline at end of file return fsolve(balance, 1e3)[0] def learn_thrust_coefficient( thrust_fn: Callable[[float], float], domain: Tuple=(1, 10000) ) -> float: speeds = np.linspace(domain[0], domain[1], num=250) thrust = np.zeros_like(speeds) for i, speed in enumerate(speeds): thrust[i] = thrust_fn(speed) return np.polyfit(speeds, thrust, deg=2)[0] # return coefficient of quadradic term def moment_of_inertia_disk(m: float, r: float) -> float: return 0.5 * m * r**2 \ No newline at end of file
 ... ... @@ -3,50 +3,62 @@ from dataclasses import dataclass import numpy as np @dataclass class MotorParams: # See: http://learningrc.com/motor-kv/, http://web.mit.edu/first/scooter/motormath.pdf k_m: float "Motor constant, where omega = k_m * back_emf, and k_m = 1/k_e" k_e: float = None "Back-EMF constant, relates motor speed induced voltage, back_emf = k_e * omega" k_q: float = None "Torque constant k_q, where torque Q = k_q * current. Equal to k_e" @dataclass class PropellerParams: moment_of_inertia: float # Manufacturer propeller length x pitch specification: diameter: float = 6 #inches pitch: float = 3 #inches a: float = 5.7 "Lift curve slope used in example in Stevens & Lewis" # TODO: read more "Lift curve slope used in example in Stevens & Lewis (eq 2.2-6a)" # d C_L / d alpha = 2 pi / (sqrt(1-M^2)), where M is mach number, alpha is # aerodynamic angle of x-axis of body and x-axis of stability b: float = 2 "Number of blades" c: float = 0.0274 "Mean chord length (m)" "Mean chord length (m) of the propeller blade" eta: float = 1. "Propeller efficiency" k_lift: float = None "Propeller's aerodynamic lift coefficient" k_drag: float = None "Propeller's aerodynamic drag coefficient" k_thrust: float = None "Propeller's aerodynamic thrust coefficient, where thrust = k_thrust * angular velocity^2" k_torque: float = None "Torque constant, where torque = k_torque * angular velocity^2" motor: MotorParams = None "The parameters of the motor to simulate speed, otherwise instantaneous." def __post_init__(self): # ensure torque function gets floats so adding float + int in the njit # torque function is not ignored self.moment_of_inertia = float(self.moment_of_inertia) 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)) "Pitch angle at root of blade" 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 "Change in pitch angle towards tip of blade" # Pitch angel is reduced to allow for even lift as blade velocity increases # with increasing radius. Assuming linear reduction from root to tip. ... ... @@ -55,19 +67,44 @@ class VehicleParams: propellers: List[PropellerParams] angles: np.ndarray "Angle (radians) of propeller arm from the positive x-axis (forward) of the body frame." distances: np.ndarray "Distance (m) of each propeller from the centre of mass." clockwise: np.ndarray = None """1 if motor spins clockwise, -1 if anti-clockwise, looking from the top. Defaults to alternating clockwise/anti-clockwise.""" mass: float = 1. inertia_matrix: np.matrix = np.eye(3) def __post_init__(self): self.distances = self.distances.astype(float) 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 # Set up control allocation matrix if thrust and motor torque constants # are provided if ( all([p.k_thrust is not None for p in self.propellers]) and \ all([p.k_torque is not None for p in self.propellers]) ): alloc = np.zeros((4, len(self.propellers))) #[Fz, Mx, My, Mz] x n-Propellers x = self.distances * np.sin(self.angles) y = self.distances * np.cos(self.angles) for i, p in enumerate(self.propellers): alloc[0, i] = -p.k_thrust # vertical force (negative up) alloc[1, i] = p.k_thrust * x[i] # torque about x-axis alloc[2, i] = p.k_thrust * y[i] # torque about y-axis alloc[3, i] = p.k_torque * self.clockwise[i] # torque about z-axis self.alloc = alloc self.alloc_inverse = np.linalg.pinv(alloc) else: self.alloc = None self.alloc_inverse = None ... ...
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