Loading .gitignore +3 −0 Original line number Diff line number Diff line __pycache__/ .ipynb_checkpoints/ .vscode/ coords.py +2 −2 Original line number Diff line number Diff line import numpy as np from numpy import sin, cos, tan, sec from numpy import sin, cos, tan def body_to_inertial(coords: np.ndarray, *, rotations=None, dcm=None, dcm_inverse=None) -> np.ndarray: Loading Loading @@ -58,7 +58,7 @@ def angular_to_euler_rate(angular_velocity: np.ndarray, orientation: np.ndarray) p, q, r = angular_velocity roll_rate = p + q * tan(pitch) * (q * sin(roll) + r * cos(roll)) pitch_rate = q * cos(roll) - r * sin(roll) yaw_rate = sec(pitch) * (q * sin(roll) + r * cos(roll)) yaw_rate = (q * sin(roll) + r * cos(roll)) / cos(pitch) return np.asarray([roll_rate, pitch_rate, yaw_rate]) Loading helpers.py 0 → 100644 +55 −0 Original line number Diff line number Diff line from typing import Iterable import numpy as np 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: coords = np.asarray(coords) masses = np.asarray(point_masses) x,y,z = coords[:,0], coords[:,1], coords[:2] Ixx = np.sum(masses * (y**2 + z**2)) Iyy = np.sum(masses * (x**2 + z**2)) Izz = np.sum(masses * (x**2 + y**2)) Ixy = Iyx = -np.sum(x * y * masses) Iyz = Izy = -np.sum(y * z * masses) Ixz = Izx = -np.sum(x * z * masses) return np.asmatrix([ [Ixx, Ixy, Ixz], [Iyx, Iyy, Iyz], [Izx, Izy, Izz] ]) 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) masses = [m_prop] * n x = np.cos(angles) * d_prop y = np.sin(angles) * d_prop z = np.zeros_like(y) coords = np.vstack((x, y, z)).T I_prop = moment_of_inertia_tensor_from_cooords(masses, coords) if body_shape == 'point': I_body = np.asmatrix(np.zeros((3,3))) elif body_shape == 'sphere_solid': I_body = np.asmatrix(np.eye(3) * 0.4 * m_body * body_size**2) elif body_shape == 'sphere_shell': I_body = np.asmatrix(np.eye(3) * 2 * m_body * body_size**2 / 3) elif body_shape == 'cube': I_body = np.asmatrix(np.eye(3) * 2 * m_body * body_size**2 / 12) I = I_body + I_prop return VehicleParams( propellers = [Propeller(params) for _ in range(n)], angles = angles, distances=np.ones(n) * d_prop, mass = n * m_prop + m_body, inertia_matrix = I ) physics.py +49 −212 Original line number Diff line number Diff line from typing import List from typing import Iterable import numpy as np from numpy import (cos, sin) from numba import njit, jit from scipy.optimize import fsolve from vehicle import PropellerParams, SimulationParams, VehicleParams from coords import rotating_frame_derivative @njit def thrustEqn(vi, *prop_params): # Unpack parameters Loading @@ -28,112 +26,37 @@ def thrustEqn(vi, *prop_params): class Propeller: def __init__(self, params: PropellerParams) -> None: self.params: PropellerParams = params self.speed: float = None "Revolutions per minute" self._last_induced_velocity = 0. "Guess for the induced velocity about the propeller" self.theta0 = 2*np.arctan2(self.params.p_pitch, (2 * np.pi * 3/4 * self.params.p_diameter/2)) self.theta1 = -4 / 3 * np.arctan2(self.params.p_pitch, 2 * np.pi * 3/4 * self.params.p_diameter/2) def step(self, u: float, dt: float) -> float: return self.apply_speed(u) def apply_speed(self, speed: float) -> float: self.speed = speed return self.speed def thrust(self, airstream_velocity: np.ndarray=(0,0,0)) -> float: def thrust(speed, airstream_velocity, R, A, rho, a, b, c, eta, theta0, theta1) -> float: u, v, w = airstream_velocity # Inputs: Current state x[k], Commanded Propeller RPM inputs u[k], # Propeller location distances dx, dy (m) # Returns: Thrust vector for 4 propellers (Newtons) # Convert commanded RPM to rad/s Omega = 2 * np.pi / 60 * self.speed Omega = 2 * np.pi / 60 * speed #Collect propeller config, state, and input parameters prop_params = (self.params.R,self.params.A,self.params.rho,self.params.a,self.params.b,self.params.c,self.params.eta,self.theta0,self.theta1,u,v,w,Omega) prop_params = (R,A,rho,a,b,c,eta,theta0,theta1,u,v,w,Omega) # Numerically solve for propeller induced velocity, vi # using nonlinear root finder, fsolve, and prop_params vi = fsolve(thrustEqn, self._last_induced_velocity, args=prop_params) self._last_induced_velocity = vi # TODO: numba jit gives error for this function ('Untyped global name fsolve') vi = fsolve(thrustEqn, 0.1, args=prop_params) # Plug vi back into Thrust equation to solve for T Vprime = np.sqrt(u**2 + v**2 + (w - vi)**2) Thrust = self.params.eta * 2 * vi * self.params.rho * self.params.A * Vprime Thrust = eta * 2 * vi * rho * A * Vprime return Thrust def torque(self, position_vector: np.ndarray, thrust: float) -> float: @njit def torque(position_vector: np.ndarray, thrust: float) -> np.ndarray: thrust = np.asarray([0, 0, -thrust]) return np.cross(position_vector, thrust) @property def state(self) -> float: return self.speed class Multirotor: def __init__(self, vehicle: VehicleParams, simulation: SimulationParams) -> None: self.vehicle: VehicleParams = vehicle self.simulation: SimulationParams = simulation self.state: np.ndarray = None self.propellers: List[Propeller] = None self.propeller_vectors: np.matrix = None def reset(self): self.propellers = [] for params in self.vehicle.propellers: self.propellers.append(Propeller(params)) x = cos(self.vehicle.angles) * self.vehicle.distances y = sin(self.vehicle.angles) * self.vehicle.distances z = np.zeros_like(y) self.propeller_vectors = np.asmatrix(np.vstack((x, y, z))) self.inertial_matrix_inverse = np.asmatrix(np.linalg.inv(self.vehicle.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]) @property def velocity(self) -> np.ndarray: """Body-frame velocity""" return self.state[:3] @property def orientation(self) -> np.ndarray: """Euler rotations (roll, pitch, yaw)""" return self.state[6:9] @property def angular_velocity(self) -> np.ndarray: """Angular rate of body frame axes (not same as rate of roll, pitch, yaw)""" return self.state[3:6] def apply_forces_torques(self, forces: np.ndarray, torques: 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): # Store state variables in a readable format x = self.state ub = x[0] vb = x[1] wb = x[2] Loading @@ -154,15 +77,15 @@ class Multirotor: fx, fy, fz = forces tx, ty, tz = torques I = self.vehicle.inertia_matrix I_inv = self.inertial_matrix_inverse I = inertia_matrix I_inv = inertia_matrix_inverse # Calculate the derivative of the state matrix using EOM xdot = np.zeros_like(self.state) xdot = np.zeros_like(x) xdot[0] = -self.simulation.g * sthe + r * vb - q * wb # = udot xdot[1] = self.simulation.g * sphi * cthe - r * ub + p * wb # = vdot xdot[2] = 1/self.vehicle.mass * (fz) + self.simulation.g * cphi * cthe + q * ub - p * vb # = wdot xdot[0] = -g * sthe + r * vb - q * wb # = udot xdot[1] = g * sphi * cthe - r * ub + p * wb # = vdot xdot[2] = 1/mass * (fz) + g * cphi * cthe + q * ub - p * vb # = wdot # xdot[3] = 1/Ixx * (tx + (Iyy - Izz) * q * r) # = pdot Loading @@ -183,89 +106,3 @@ class Multirotor: xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot return xdot def apply_propeller_speeds(self, speeds: np.ndarray): linear_vel_body = self.state[:3] angular_vel_body = self.state[3:6] airstream_velocity_inertial = rotating_frame_derivative( self.propeller_vectors, linear_vel_body, angular_vel_body) thrust = np.zeros(3, len(self.propellers)) torque = np.zeros_like(thrust) for i, speed, prop in enumerate(zip( speeds, self.propellers)): speed = prop.apply_speed(speed) thrust[2, i] = -prop.thrust(airstream_velocity_inertial[:, i]) torque[:, i] = prop.torque(self.propeller_vectors[:,i], -thrust[2:,i]) forces = thrust.sum(axis=1) torques = np.cross(self.propeller_vectors, thrust) return self.apply_forces_torques(forces, torques) def step(self): pass def stateDerivative(x,u,physics=None,geometry=None): # Inputs: state vector (x), input vector (u) # Returns: time derivative of state vector (xdot) # State Vector Reference: #idx 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 #x = [u, v, w, p, q, r, phi, the, psi, xE, yE, hE] # Store state variables in a readable format ub = x[0] vb = x[1] wb = x[2] p = x[3] q = x[4] r = x[5] phi = x[6] theta = x[7] psi = x[8] xE = x[9] yE = x[10] hE = x[11] # Calculate forces and moments from propeller inputs (u) F1 = fthrust(x, u[0], dx, dy) F2 = fthrust(x, u[1], -dx, -dy) F3 = fthrust(x, u[2], dx, -dy) F4 = fthrust(x, u[3], -dx, dy) Fz = F1 + F2 + F3 + F4 L = (F2 + F3) * dy - (F1 + F4) * dy M = (F1 + F3) * dx - (F2 + F4) * dx N = -T(F1,dx,dy) - T(F2,dx,dy) + T(F3,dx,dy) + T(F4,dx,dy) # Pre-calculate trig values cphi = np.cos(phi); sphi = np.sin(phi) cthe = np.cos(theta); sthe = np.sin(theta) cpsi = np.cos(psi); spsi = np.sin(psi) # Calculate the derivative of the state matrix using EOM xdot = np.zeros(12) xdot[0] = -g * sthe + r * vb - q * wb # = udot xdot[1] = g * sphi*cthe - r * ub + p * wb # = vdot xdot[2] = 1/m * (-Fz) + g*cphi*cthe + q * ub - p * vb # = wdot xdot[3] = 1/Ixx * (L + (Iyy - Izz) * q * r) # = pdot xdot[4] = 1/Iyy * (M + (Izz - Ixx) * p * r) # = qdot xdot[5] = 1/Izz * (N + (Ixx - Iyy) * p * q) # = rdot xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe # = phidot xdot[7] = q * cphi - r * sphi # = thetadot xdot[8] = (q * sphi + r * cphi) / cthe # = psidot xdot[9] = cthe*cpsi*ub + (-cphi*spsi + sphi*sthe*cpsi) * vb + \ (sphi*spsi+cphi*sthe*cpsi) * wb # = xEdot xdot[10] = cthe*spsi * ub + (cphi*cpsi+sphi*sthe*spsi) * vb + \ (-sphi*cpsi+cphi*sthe*spsi) * wb # = yEdot xdot[11] = -1*(-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = hEdot return xdot simulation.py 0 → 100644 +127 −0 Original line number Diff line number Diff line 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 physics import thrust, torque, apply_forces_torques class Propeller: def __init__(self, params: PropellerParams, simulation: SimulationParams) -> None: self.params: PropellerParams = params self.simulation: SimulationParams = simulation self.speed: float = None "Revolutions per minute" self._last_induced_velocity = 0.1 "Guess for the induced velocity about the propeller" def reset(self): self.speed = 0. self._last_induced_velocity = 0.1 def step(self, u: float, dt: float) -> float: return self.apply_speed(u) def apply_speed(self, speed: float) -> float: self.speed = speed return self.speed @property def state(self) -> float: return self.speed class Multirotor: def __init__(self, params: VehicleParams, simulation: SimulationParams) -> None: self.params: VehicleParams = params self.simulation: SimulationParams = simulation self.state: np.ndarray = None self.propellers: List[Propeller] = None self.propeller_vectors: np.matrix = None 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]) @property def velocity(self) -> np.ndarray: """Body-frame velocity""" return self.state[:3] @property def orientation(self) -> np.ndarray: """Euler rotations (roll, pitch, yaw)""" return self.state[6:9] @property def angular_velocity(self) -> np.ndarray: """Angular rate of body frame axes (not same as rate of roll, pitch, yaw)""" return self.state[3:6] @property def euler_rate(self) -> np.ndarray: """Euler rate of vehicle d(roll, pitch, yaw)/dt""" return angular_to_euler_rate(self.angular_velocity, self.orientation) def get_forces_torques(self, speeds: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: linear_vel_body = self.state[:3] angular_vel_body = self.state[3:6] airstream_velocity_inertial = rotating_frame_derivative( self.propeller_vectors, linear_vel_body, angular_vel_body) thrust_vec = np.zeros((3, len(self.propellers))) torque_vec = np.zeros_like(thrust_vec) for i, (speed, prop) in enumerate(zip( 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) torque_vec[:, i] = torque(self.propeller_vectors[:,i], -thrust_vec[2,i]) forces = thrust_vec.sum(axis=1) torques = torque_vec.sum(axis=1) return forces, torques 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) return xdot def step(self, u: np.ndarray, jit=False): self.state = odeint(self.dxdt, self.state, (0, self.simulation.dt), args=(u,))[-1] return self.state Loading
.gitignore +3 −0 Original line number Diff line number Diff line __pycache__/ .ipynb_checkpoints/ .vscode/
coords.py +2 −2 Original line number Diff line number Diff line import numpy as np from numpy import sin, cos, tan, sec from numpy import sin, cos, tan def body_to_inertial(coords: np.ndarray, *, rotations=None, dcm=None, dcm_inverse=None) -> np.ndarray: Loading Loading @@ -58,7 +58,7 @@ def angular_to_euler_rate(angular_velocity: np.ndarray, orientation: np.ndarray) p, q, r = angular_velocity roll_rate = p + q * tan(pitch) * (q * sin(roll) + r * cos(roll)) pitch_rate = q * cos(roll) - r * sin(roll) yaw_rate = sec(pitch) * (q * sin(roll) + r * cos(roll)) yaw_rate = (q * sin(roll) + r * cos(roll)) / cos(pitch) return np.asarray([roll_rate, pitch_rate, yaw_rate]) Loading
helpers.py 0 → 100644 +55 −0 Original line number Diff line number Diff line from typing import Iterable import numpy as np 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: coords = np.asarray(coords) masses = np.asarray(point_masses) x,y,z = coords[:,0], coords[:,1], coords[:2] Ixx = np.sum(masses * (y**2 + z**2)) Iyy = np.sum(masses * (x**2 + z**2)) Izz = np.sum(masses * (x**2 + y**2)) Ixy = Iyx = -np.sum(x * y * masses) Iyz = Izy = -np.sum(y * z * masses) Ixz = Izx = -np.sum(x * z * masses) return np.asmatrix([ [Ixx, Ixy, Ixz], [Iyx, Iyy, Iyz], [Izx, Izy, Izz] ]) 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) masses = [m_prop] * n x = np.cos(angles) * d_prop y = np.sin(angles) * d_prop z = np.zeros_like(y) coords = np.vstack((x, y, z)).T I_prop = moment_of_inertia_tensor_from_cooords(masses, coords) if body_shape == 'point': I_body = np.asmatrix(np.zeros((3,3))) elif body_shape == 'sphere_solid': I_body = np.asmatrix(np.eye(3) * 0.4 * m_body * body_size**2) elif body_shape == 'sphere_shell': I_body = np.asmatrix(np.eye(3) * 2 * m_body * body_size**2 / 3) elif body_shape == 'cube': I_body = np.asmatrix(np.eye(3) * 2 * m_body * body_size**2 / 12) I = I_body + I_prop return VehicleParams( propellers = [Propeller(params) for _ in range(n)], angles = angles, distances=np.ones(n) * d_prop, mass = n * m_prop + m_body, inertia_matrix = I )
physics.py +49 −212 Original line number Diff line number Diff line from typing import List from typing import Iterable import numpy as np from numpy import (cos, sin) from numba import njit, jit from scipy.optimize import fsolve from vehicle import PropellerParams, SimulationParams, VehicleParams from coords import rotating_frame_derivative @njit def thrustEqn(vi, *prop_params): # Unpack parameters Loading @@ -28,112 +26,37 @@ def thrustEqn(vi, *prop_params): class Propeller: def __init__(self, params: PropellerParams) -> None: self.params: PropellerParams = params self.speed: float = None "Revolutions per minute" self._last_induced_velocity = 0. "Guess for the induced velocity about the propeller" self.theta0 = 2*np.arctan2(self.params.p_pitch, (2 * np.pi * 3/4 * self.params.p_diameter/2)) self.theta1 = -4 / 3 * np.arctan2(self.params.p_pitch, 2 * np.pi * 3/4 * self.params.p_diameter/2) def step(self, u: float, dt: float) -> float: return self.apply_speed(u) def apply_speed(self, speed: float) -> float: self.speed = speed return self.speed def thrust(self, airstream_velocity: np.ndarray=(0,0,0)) -> float: def thrust(speed, airstream_velocity, R, A, rho, a, b, c, eta, theta0, theta1) -> float: u, v, w = airstream_velocity # Inputs: Current state x[k], Commanded Propeller RPM inputs u[k], # Propeller location distances dx, dy (m) # Returns: Thrust vector for 4 propellers (Newtons) # Convert commanded RPM to rad/s Omega = 2 * np.pi / 60 * self.speed Omega = 2 * np.pi / 60 * speed #Collect propeller config, state, and input parameters prop_params = (self.params.R,self.params.A,self.params.rho,self.params.a,self.params.b,self.params.c,self.params.eta,self.theta0,self.theta1,u,v,w,Omega) prop_params = (R,A,rho,a,b,c,eta,theta0,theta1,u,v,w,Omega) # Numerically solve for propeller induced velocity, vi # using nonlinear root finder, fsolve, and prop_params vi = fsolve(thrustEqn, self._last_induced_velocity, args=prop_params) self._last_induced_velocity = vi # TODO: numba jit gives error for this function ('Untyped global name fsolve') vi = fsolve(thrustEqn, 0.1, args=prop_params) # Plug vi back into Thrust equation to solve for T Vprime = np.sqrt(u**2 + v**2 + (w - vi)**2) Thrust = self.params.eta * 2 * vi * self.params.rho * self.params.A * Vprime Thrust = eta * 2 * vi * rho * A * Vprime return Thrust def torque(self, position_vector: np.ndarray, thrust: float) -> float: @njit def torque(position_vector: np.ndarray, thrust: float) -> np.ndarray: thrust = np.asarray([0, 0, -thrust]) return np.cross(position_vector, thrust) @property def state(self) -> float: return self.speed class Multirotor: def __init__(self, vehicle: VehicleParams, simulation: SimulationParams) -> None: self.vehicle: VehicleParams = vehicle self.simulation: SimulationParams = simulation self.state: np.ndarray = None self.propellers: List[Propeller] = None self.propeller_vectors: np.matrix = None def reset(self): self.propellers = [] for params in self.vehicle.propellers: self.propellers.append(Propeller(params)) x = cos(self.vehicle.angles) * self.vehicle.distances y = sin(self.vehicle.angles) * self.vehicle.distances z = np.zeros_like(y) self.propeller_vectors = np.asmatrix(np.vstack((x, y, z))) self.inertial_matrix_inverse = np.asmatrix(np.linalg.inv(self.vehicle.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]) @property def velocity(self) -> np.ndarray: """Body-frame velocity""" return self.state[:3] @property def orientation(self) -> np.ndarray: """Euler rotations (roll, pitch, yaw)""" return self.state[6:9] @property def angular_velocity(self) -> np.ndarray: """Angular rate of body frame axes (not same as rate of roll, pitch, yaw)""" return self.state[3:6] def apply_forces_torques(self, forces: np.ndarray, torques: 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): # Store state variables in a readable format x = self.state ub = x[0] vb = x[1] wb = x[2] Loading @@ -154,15 +77,15 @@ class Multirotor: fx, fy, fz = forces tx, ty, tz = torques I = self.vehicle.inertia_matrix I_inv = self.inertial_matrix_inverse I = inertia_matrix I_inv = inertia_matrix_inverse # Calculate the derivative of the state matrix using EOM xdot = np.zeros_like(self.state) xdot = np.zeros_like(x) xdot[0] = -self.simulation.g * sthe + r * vb - q * wb # = udot xdot[1] = self.simulation.g * sphi * cthe - r * ub + p * wb # = vdot xdot[2] = 1/self.vehicle.mass * (fz) + self.simulation.g * cphi * cthe + q * ub - p * vb # = wdot xdot[0] = -g * sthe + r * vb - q * wb # = udot xdot[1] = g * sphi * cthe - r * ub + p * wb # = vdot xdot[2] = 1/mass * (fz) + g * cphi * cthe + q * ub - p * vb # = wdot # xdot[3] = 1/Ixx * (tx + (Iyy - Izz) * q * r) # = pdot Loading @@ -183,89 +106,3 @@ class Multirotor: xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot return xdot def apply_propeller_speeds(self, speeds: np.ndarray): linear_vel_body = self.state[:3] angular_vel_body = self.state[3:6] airstream_velocity_inertial = rotating_frame_derivative( self.propeller_vectors, linear_vel_body, angular_vel_body) thrust = np.zeros(3, len(self.propellers)) torque = np.zeros_like(thrust) for i, speed, prop in enumerate(zip( speeds, self.propellers)): speed = prop.apply_speed(speed) thrust[2, i] = -prop.thrust(airstream_velocity_inertial[:, i]) torque[:, i] = prop.torque(self.propeller_vectors[:,i], -thrust[2:,i]) forces = thrust.sum(axis=1) torques = np.cross(self.propeller_vectors, thrust) return self.apply_forces_torques(forces, torques) def step(self): pass def stateDerivative(x,u,physics=None,geometry=None): # Inputs: state vector (x), input vector (u) # Returns: time derivative of state vector (xdot) # State Vector Reference: #idx 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 #x = [u, v, w, p, q, r, phi, the, psi, xE, yE, hE] # Store state variables in a readable format ub = x[0] vb = x[1] wb = x[2] p = x[3] q = x[4] r = x[5] phi = x[6] theta = x[7] psi = x[8] xE = x[9] yE = x[10] hE = x[11] # Calculate forces and moments from propeller inputs (u) F1 = fthrust(x, u[0], dx, dy) F2 = fthrust(x, u[1], -dx, -dy) F3 = fthrust(x, u[2], dx, -dy) F4 = fthrust(x, u[3], -dx, dy) Fz = F1 + F2 + F3 + F4 L = (F2 + F3) * dy - (F1 + F4) * dy M = (F1 + F3) * dx - (F2 + F4) * dx N = -T(F1,dx,dy) - T(F2,dx,dy) + T(F3,dx,dy) + T(F4,dx,dy) # Pre-calculate trig values cphi = np.cos(phi); sphi = np.sin(phi) cthe = np.cos(theta); sthe = np.sin(theta) cpsi = np.cos(psi); spsi = np.sin(psi) # Calculate the derivative of the state matrix using EOM xdot = np.zeros(12) xdot[0] = -g * sthe + r * vb - q * wb # = udot xdot[1] = g * sphi*cthe - r * ub + p * wb # = vdot xdot[2] = 1/m * (-Fz) + g*cphi*cthe + q * ub - p * vb # = wdot xdot[3] = 1/Ixx * (L + (Iyy - Izz) * q * r) # = pdot xdot[4] = 1/Iyy * (M + (Izz - Ixx) * p * r) # = qdot xdot[5] = 1/Izz * (N + (Ixx - Iyy) * p * q) # = rdot xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe # = phidot xdot[7] = q * cphi - r * sphi # = thetadot xdot[8] = (q * sphi + r * cphi) / cthe # = psidot xdot[9] = cthe*cpsi*ub + (-cphi*spsi + sphi*sthe*cpsi) * vb + \ (sphi*spsi+cphi*sthe*cpsi) * wb # = xEdot xdot[10] = cthe*spsi * ub + (cphi*cpsi+sphi*sthe*spsi) * vb + \ (-sphi*cpsi+cphi*sthe*spsi) * wb # = yEdot xdot[11] = -1*(-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = hEdot return xdot
simulation.py 0 → 100644 +127 −0 Original line number Diff line number Diff line 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 physics import thrust, torque, apply_forces_torques class Propeller: def __init__(self, params: PropellerParams, simulation: SimulationParams) -> None: self.params: PropellerParams = params self.simulation: SimulationParams = simulation self.speed: float = None "Revolutions per minute" self._last_induced_velocity = 0.1 "Guess for the induced velocity about the propeller" def reset(self): self.speed = 0. self._last_induced_velocity = 0.1 def step(self, u: float, dt: float) -> float: return self.apply_speed(u) def apply_speed(self, speed: float) -> float: self.speed = speed return self.speed @property def state(self) -> float: return self.speed class Multirotor: def __init__(self, params: VehicleParams, simulation: SimulationParams) -> None: self.params: VehicleParams = params self.simulation: SimulationParams = simulation self.state: np.ndarray = None self.propellers: List[Propeller] = None self.propeller_vectors: np.matrix = None 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]) @property def velocity(self) -> np.ndarray: """Body-frame velocity""" return self.state[:3] @property def orientation(self) -> np.ndarray: """Euler rotations (roll, pitch, yaw)""" return self.state[6:9] @property def angular_velocity(self) -> np.ndarray: """Angular rate of body frame axes (not same as rate of roll, pitch, yaw)""" return self.state[3:6] @property def euler_rate(self) -> np.ndarray: """Euler rate of vehicle d(roll, pitch, yaw)/dt""" return angular_to_euler_rate(self.angular_velocity, self.orientation) def get_forces_torques(self, speeds: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: linear_vel_body = self.state[:3] angular_vel_body = self.state[3:6] airstream_velocity_inertial = rotating_frame_derivative( self.propeller_vectors, linear_vel_body, angular_vel_body) thrust_vec = np.zeros((3, len(self.propellers))) torque_vec = np.zeros_like(thrust_vec) for i, (speed, prop) in enumerate(zip( 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) torque_vec[:, i] = torque(self.propeller_vectors[:,i], -thrust_vec[2,i]) forces = thrust_vec.sum(axis=1) torques = torque_vec.sum(axis=1) return forces, torques 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) return xdot def step(self, u: np.ndarray, jit=False): self.state = odeint(self.dxdt, self.state, (0, self.simulation.dt), args=(u,))[-1] return self.state
