Commit 0944ee8c authored by Ibrahim's avatar Ibrahim
Browse files

simulation/physics: making dxdt() method pure for integration and adding...

simulation/physics: making dxdt() method pure for integration and adding torque of spinning propellers
parent 91fa1e7a
......@@ -10,8 +10,8 @@ from scipy.optimize import fsolve
def thrustEqn(vi, *prop_params):
R,A,rho,a,b,c,eta,theta0,theta1,u,v,w,Omega = prop_params
# Calculate local airflow velocity at propeller with vi, V'
Vprime = np.sqrt(u**2 + v**2 + (w - vi)**2)
# Calculate local airflow velocity at propeller with vi, v'
vprime = np.sqrt(u**2 + v**2 + (w - vi)**2)
# Calculate Thrust averaged over one revolution of propeller using vi
Thrust = 1/4 * rho * a * b * c * R * \
......@@ -19,38 +19,49 @@ def thrustEqn(vi, *prop_params):
(u**2 + v**2) * (theta0 + 1/2 * theta1) )
# Calculate residual for equation: Thrust = mass flow rate * delta Velocity
residual = eta * 2 * vi * rho * A * Vprime - Thrust
residual = eta * 2 * vi * rho * A * vprime - Thrust
return residual
def thrust(
speed, airstream_velocity, R, A, rho, a, b, c, eta, theta0, theta1,
vi_guess=0.1
prop_speed, airstream_velocity, R, A, rho, a, b, c, eta, theta0, theta1
) -> float:
u, v, w = airstream_velocity
# Convert commanded RPM to rad/s
Omega = 2 * np.pi / 60 * speed
# omega = 2 * np.pi / 60 * prop_speed # Commented out - already in rad/s
omega = prop_speed
vi_guess = airstream_velocity[2]
#Collect propeller config, state, and input parameters
prop_params = (R,A,rho,a,b,c,eta,theta0,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
# TODO: numba jit gives error for fsolve ('Untyped global name fsolve')
vi = fsolve(thrustEqn, vi_guess, args=prop_params)
vi = fsolve(thrustEqn, vi_guess, args=prop_params, xtol=1e-4)[0]
# Plug vi back into Thrust equation to solve for T
Vprime = np.sqrt(u**2 + v**2 + (w - vi)**2)
Thrust = eta * 2 * vi * rho * A * Vprime
return Thrust
vprime = np.sqrt(u**2 + v**2 + (w - vi)**2)
thrust = eta * 2 * vi * rho * A * vprime
return thrust
@njit
def torque(position_vector: np.ndarray, thrust: float) -> np.ndarray:
thrust = np.asarray([0, 0, -thrust])
return np.cross(position_vector, thrust)
def torque(
position_vector: np.ndarray, force: np.ndarray,
moment_of_inertia: float, prop_angular_acceleration: float
) -> np.ndarray:
# Total moments in the body frame
# yaw moments
# tau = I . d omega/dt
tau_rot = moment_of_inertia * prop_angular_acceleration
# tau = r x F
tau = np.cross(position_vector, force)
# print(moment_of_inertia, prop_angular_acceleration)
tau[2] = tau[2] + tau_rot
return tau
......@@ -74,9 +85,9 @@ def apply_forces_torques(
zI = x[11] # In inertial frame, down is positive z
# 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)
cphi = np.cos(phi); sphi = np.sin(phi) # roll
cthe = np.cos(theta); sthe = np.sin(theta) # pitch
cpsi = np.cos(psi); spsi = np.sin(psi) # yaw
fx, fy, fz = forces
tx, ty, tz = torques
......@@ -86,15 +97,18 @@ def apply_forces_torques(
# Calculate the derivative of the state matrix using EOM
xdot = np.zeros_like(x)
xdot[0] = -g * sthe + r * vb - q * wb # = udot
xdot[1] = g * sphi * cthe - r * ub + p * wb # = vdot
xdot[0] = 1/mass * (fx) - g * sthe + r * vb - q * wb # = udot
xdot[1] = 1/mass * (fy) + 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
# xdot[4] = 1/Iyy * (ty + (Izz - Ixx) * p * r) # = qdot
# xdot[5] = 1/Izz * (tz + (Ixx - Iyy) * p * q) # = rdot
xdot[3:6] = I_inv @ (torques - np.cross(x[3:6], I @ x[3:6]))
# print(I.shape, x[3:6].shape, (I @ x[3:6]).shape)
gyro = np.cross(x[3:6], I @ x[3:6])
# gyro = np.cross(x[3:6], I @ x[3:6]).squeeze()
xdot[3:6] = I_inv @ (torques - gyro) # TODO: verify whether it is + or - gyro
xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe # = phidot
xdot[7] = q * cphi - r * sphi # = thetadot
......
......@@ -4,26 +4,27 @@ import numpy as np
from numpy import (cos, sin)
from scipy.integrate import odeint
from vehicle import PropellerParams, SimulationParams, VehicleParams
from coords import body_to_inertial, rotating_frame_derivative, angular_to_euler_rate
from vehicle import MotorParams, PropellerParams, SimulationParams, VehicleParams
from coords import body_to_inertial, direction_cosine_matrix, rotating_frame_derivative, angular_to_euler_rate
from physics import thrust, torque, apply_forces_torques
class Propeller:
"""
Models the thrust and aerodynamics of the propeller blades spinning at a
certain rate.
"""
def __init__(self, params: PropellerParams, simulation: SimulationParams) -> None:
self.params: PropellerParams = params
self.simulation: SimulationParams = simulation
self.speed: float = None
"Revolutions per minute"
self._induced_velocity_guess = 0.1
"Guess for the induced velocity about the propeller"
self.speed: float = 0.
"Radians per second"
def reset(self):
self.speed = 0.
self._induced_velocity_guess = 0.1
def step(self, u: float) -> float:
......@@ -31,17 +32,19 @@ class Propeller:
def apply_speed(self, speed: float) -> float:
return speed
def step(self, speed: float) -> float:
self.speed = speed
return self.speed
def thrust(self, speed=None, airstream_velocity: np.ndarray=np.zeros(3)) -> float:
def thrust(self, speed, 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)
p.R, p.A, self.simulation.rho, p.a, p.b, p.c, p.eta, p.theta0, p.theta1
)
@property
......@@ -49,6 +52,33 @@ class Propeller:
return self.speed
@property
def rpm(self) -> float:
return self.speed * 60. / (2. * np.pi)
class Motor:
"""
Models the electronic and mechanical characterists of the rotation of the
propeller shaft.
"""
def __init__(self, params: MotorParams, simulation: SimulationParams) -> None:
self.params = params
self.simulation = simulation
self.speed: float = 0.
"Radians per second"
def reset(self) -> float:
self.speed = 0.
def step(self, u: float) -> float:
pass
class Multirotor:
......@@ -58,7 +88,7 @@ class Multirotor:
self.state: np.ndarray = None
self.propellers: List[Propeller] = None
self.propeller_vectors: np.matrix = None
self.t: float = None
self.t: float = 0.
self.reset()
......@@ -92,7 +122,8 @@ class Multirotor:
@property
def navigation_velocity(self) -> np.ndarray:
v_inertial = body_to_inertial(self.velocity)
dcm = direction_cosine_matrix(*self.orientation)
v_inertial = body_to_inertial(self.velocity, dcm)
v_inertial[2] *= -1 # convert inertial to navigation frame (h = -z)
return v_inertial
......@@ -104,7 +135,7 @@ class Multirotor:
@property
def angular_velocity(self) -> np.ndarray:
def angular_rate(self) -> np.ndarray:
"""Angular rate of body frame axes (not same as rate of roll, pitch, yaw)"""
return self.state[3:6]
......@@ -112,12 +143,17 @@ class Multirotor:
@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)
return angular_to_euler_rate(self.angular_rate, self.orientation)
@property
def weight(self) -> float:
return self.simulation.g * self.params.mass
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]
def get_forces_torques(self, speeds: np.ndarray, state: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
linear_vel_body = state[:3]
angular_vel_body = state[3:6]
airstream_velocity_inertial = rotating_frame_derivative(
self.propeller_vectors,
linear_vel_body,
......@@ -126,29 +162,37 @@ class Multirotor:
thrust_vec = np.zeros((3, len(self.propellers)))
torque_vec = np.zeros_like(thrust_vec)
for i, (speed, prop) in enumerate(zip(
for i, (speed, prop, clockwise) in enumerate(zip(
speeds,
self.propellers)):
self.propellers,
self.params.clockwise)
):
last_speed = prop.speed
speed = prop.apply_speed(speed)
angular_acc = (speed - last_speed) / self.simulation.dt
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])
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[:,i],
prop.params.moment_of_inertia, clockwise * angular_acc
)
# print(clockwise * angular_acc)
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)
# This method must not have any side-effects. It should not change the
# state of the vehicle. This method is called multiple times from the
# same state by the odeint() function, and the results should be consistent.
forces, torques = self.get_forces_torques(u, x)
xdot = apply_forces_torques(
forces, torques, self.state, self.simulation.g,
forces, torques, x, self.simulation.g,
self.params.mass, self.params.inertia_matrix, self.params.inertia_matrix_inverse)
return xdot
......@@ -156,4 +200,16 @@ class Multirotor:
def step(self, u: np.ndarray):
self.t += self.simulation.dt
self.state = odeint(self.dxdt, self.state, (0, self.simulation.dt), args=(u,))[-1]
for u_, prop in zip(u, self.propellers):
prop.step(u_)
return self.state
def allocate_control(self, thrust: float, torques: np.ndarray) -> np.ndarray:
# TODO: njit it? np.linalg.lstsq can be compiled
vec = np.asarray([-thrust, *torques])
# return np.sqrt(np.linalg.lstsq(self.params.alloc, vec, rcond=None)[0])
return np.sqrt(
np.clip(self.params.alloc_inverse @ vec, a_min=0., a_max=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