Spaces:
Running
on
CPU Upgrade
Running
on
CPU Upgrade
| # -*- coding: utf-8 -*- | |
| """ | |
| Created on Tue Oct 19 18:29:10 2021 | |
| @author: 2913452 | |
| TODO: | |
| - height of athlete, optimal angle shot put | |
| - note that biomech can influence | |
| how much force an athlete can emit for each angle | |
| - | |
| """ | |
| from abc import ABC, abstractmethod | |
| from scipy.integrate import solve_ivp | |
| from scipy.interpolate import interp1d | |
| from .transforms import T_12, T_23, T_34, T_14, T_41, T_31 | |
| import matplotlib.pyplot as pl | |
| from numpy import exp,matmul,pi,sqrt,arctan2,radians,degrees,sin,cos,array,concatenate,linspace,zeros_like,cross,zeros,argmin | |
| from numpy.linalg import norm | |
| from . import environment | |
| import os | |
| import yaml | |
| T_END = 60 | |
| N_STEP = 200 | |
| def hit_ground(t, y, *args): | |
| return y[2] | |
| def stopped(t, y, *args): | |
| U = norm(y[3:6]) | |
| return U - 1e-4 | |
| class Shot: | |
| def __init__(self,t,x,v,att=None): | |
| self.time = t | |
| self.position = x | |
| self.velocity = v | |
| if att is not None: | |
| self.attitude = att | |
| class _Projectile(ABC): | |
| def __init__(self): | |
| pass | |
| def _shoot(self, advance_function, y0, *args): | |
| hit_ground.terminal = True | |
| hit_ground.direction = -1 | |
| stopped.terminal = True | |
| stopped.direction = -1 | |
| sol = solve_ivp(advance_function,[0,T_END],y0, | |
| dense_output=True,args=args, | |
| method='RK45', | |
| events=(hit_ground,stopped)) | |
| t = linspace(0,sol.t[-1],N_STEP) | |
| f = sol.sol(t) | |
| pos = array([f[0],f[1],f[2]]) | |
| vel = array([f[3],f[4],f[5]]) | |
| if len(f) <= 6: | |
| shot = Shot(t, pos, vel) | |
| else: | |
| att = array([f[6],f[7],f[8]]) | |
| shot = Shot(t, pos, vel, att) | |
| return shot | |
| def advance(self,t,vec,*args): | |
| """ | |
| :param float T: Thrust | |
| :param float Q: Torque | |
| :param float P: Power | |
| :return: Right hand side of kinematic equations for a projectile | |
| :rtype: array | |
| """ | |
| class _Particle(_Projectile): | |
| def __init__(self): | |
| super().__init__() | |
| self.g = environment.g | |
| def initialize_shot(self, **kwargs): | |
| kwargs.setdefault('yaw', 0.0) | |
| pitch = radians(kwargs["pitch"]) | |
| yaw = radians(kwargs["yaw"]) | |
| U = kwargs["speed"] | |
| xy = cos(pitch) | |
| u = U*xy*cos(yaw) | |
| w = U*sin(pitch) | |
| v = U*xy*sin(-yaw) | |
| if "position" in kwargs: | |
| x,y,z = kwargs["position"] | |
| else: | |
| x = 0. | |
| y = 0. | |
| z = 0. | |
| y0 = array((x,y,z,u,v,w)) | |
| return y0 | |
| def shoot(self, **kwargs): | |
| y0 = self.initialize_shot(**kwargs) | |
| shot = self._shoot(self.advance, y0) | |
| return shot | |
| def gravity_force(self, x=None): | |
| if x is None: | |
| return array((0,0,environment.g)) | |
| else: | |
| # Messy way to return g array... | |
| f = zeros_like(x) | |
| f[0,:] = 0 | |
| f[1,:] = 0 | |
| f[2,:] = environment.g | |
| return f | |
| def advance(self, t, vec, *args): | |
| # x, y, z, u, v, w = vec | |
| x = vec[0:3] | |
| u = vec[3:6] | |
| f = self.gravity_force() | |
| return concatenate((u,f)) | |
| class _SphericalParticleAirResistance(_Particle): | |
| def __init__(self, mass, diameter): | |
| super().__init__() | |
| self.mass = mass | |
| self.diameter = diameter | |
| self.radius = 0.5*diameter | |
| self.area = 0.25*pi*diameter**2 | |
| self.volume = 4./3.*pi*self.radius**3 | |
| def air_resistance_force(self, U, Cd): | |
| f = -0.5*environment.rho*self.area*Cd*norm(U)*U/self.mass | |
| #f = -0.5*environment.rho*self.area*Cd*Umag*U/self.mass | |
| return f | |
| def advance(self, t, vec, *args): | |
| x = vec[0:3] | |
| u = vec[3:6] | |
| Cd = self.drag_coefficient(norm(u)) | |
| f = self.air_resistance_force(u, Cd) \ | |
| + self.gravity_force() | |
| return concatenate((u,f)) | |
| def reynolds_number(self, velocity): | |
| """ | |
| Reynolds number, non-dimensional number giving the | |
| ratio of inertial forces to viscous forces. Used | |
| for calculating the drag coefficient. | |
| :param float velocity: Velocity seen by particle | |
| :return: Reynolds number | |
| :rtype: float | |
| """ | |
| return environment.rho*velocity*self.diameter/environment.mu | |
| def drag_coefficient(self, velocity): | |
| """ | |
| Drag coefficient for sphere, empirical curve fit | |
| taken from: | |
| F. A. Morrison, An Introduction to Fluid Mechanics, (Cambridge | |
| University Press, New York, 2013). This correlation appears in | |
| Figure 8.13 on page 625. | |
| The full formula is: | |
| .. math:: | |
| F = \\frac{2}{\\pi}\\cos^{-1}e^{-f} \\\\ | |
| f = \\frac{B}{2}\\frac{R-r}{r\\sin\\phi} | |
| :param float velocity: Velocity seen by particle | |
| :return: Drag coefficient | |
| :rtype: float | |
| """ | |
| Re = self.reynolds_number(velocity) | |
| if Re <= 0: | |
| return 1e30 | |
| tmp1 = Re/5.0 | |
| tmp2 = Re/2.63e5 | |
| tmp3 = Re/1e6 | |
| Cd = 24.0/Re \ | |
| + 2.6*tmp1/(1 + tmp1**1.52) \ | |
| + 0.411*tmp2**-7.94/(1 + tmp2**-8) \ | |
| + 0.25*tmp3/(1 + tmp3) | |
| return Cd | |
| class _SphericalParticleAirResistanceSpin(_SphericalParticleAirResistance): | |
| def __init__(self, mass, diameter): | |
| super().__init__(mass, diameter) | |
| def lift_coefficient(self, Umag, omega): | |
| # TODO - complex dependency on Re. For now, | |
| # assume constant | |
| return 0.9 | |
| def shoot(self, **kwargs): | |
| y0 = self.initialize_shot(**kwargs) | |
| spin = array((kwargs["spin"])) | |
| shot = self._shoot(self.advance, y0, spin) | |
| return shot | |
| def spin_force(self,U,spin): | |
| Umag = norm(U) | |
| omega = norm(spin) | |
| Cl = self.lift_coefficient(Umag, omega) | |
| if U.ndim == 1: | |
| f = Cl*pi*self.radius**3*environment.rho*cross(spin, U)/self.mass | |
| else: | |
| # Messy way to return spin array for post-processing | |
| f = zeros_like(U) | |
| for i in range(U.shape[1]): | |
| f[:,i] = Cl*pi*self.radius**3*environment.rho*cross(spin, U[:,i])/self.mass | |
| return f | |
| def advance(self, t, vec, spin): | |
| x = vec[0:3] | |
| u = vec[3:6] | |
| Cd = self.drag_coefficient(norm(u), norm(spin)) | |
| f = self.air_resistance_force(u, Cd) \ | |
| + self.gravity_force() \ | |
| + self.spin_force(u,spin) | |
| return concatenate((u,f)) | |
| class ShotPutBall(_SphericalParticleAirResistance): | |
| """ | |
| Note that diameter can vary 110 mm to 130mm | |
| and 95 mm to 110 mm | |
| """ | |
| def __init__(self, weight_class): | |
| if weight_class == 'M': | |
| mass = 7.26 | |
| diameter = 0.11 | |
| elif weight_class == 'F': | |
| mass = 4.0 | |
| diameter = 0.095 | |
| super().__init__(mass, diameter) | |
| class SoccerBall(_SphericalParticleAirResistanceSpin): | |
| """ | |
| Note that diameter can vary 110 mm to 130mm | |
| and 95 mm to 110 mm | |
| """ | |
| def __init__(self, mass=0.430, diameter=0.22): | |
| super(SoccerBall, self).__init__(mass, diameter) | |
| def drag_coefficient(self, velocity, omega): | |
| # Texture, sewing pattern and spin will alter | |
| # the drag coefficient. | |
| # Here, use correlation from | |
| # Goff, J. E., & Carré, M. J. (2010). Soccer ball lift | |
| # coefficients via trajectory analysis. | |
| # European Journal of Physics, 31(4), 775. | |
| vc = 12.19 | |
| vs = 1.309 | |
| S = omega*self.radius/velocity; | |
| if S > 0.05 and velocity > vc: | |
| Cd = 0.4127*S**0.3056; | |
| else: | |
| Cd = 0.155 + 0.346 / (1 + exp((velocity - vc)/vs)) | |
| return Cd | |
| def lift_coefficient(self, Umag, omega): | |
| # TODO - complex dependency on Re and spin, skin texture etc | |
| return 0.9 | |
| class TableTennisBall(SoccerBall): | |
| """ | |
| """ | |
| def __init__(self): | |
| mass = 2.7e-3 | |
| diameter = 40e-3 | |
| super(TableTennisBall, self).__init__(mass, diameter) | |
| class DiscGolfDisc(_Projectile): | |
| def __init__(self, name, mass=0.175): | |
| this_dir = os.path.dirname(os.path.abspath(__file__)) | |
| path = os.path.join(this_dir, 'discs', name + '.yaml') | |
| self.name = name | |
| with open(path, 'r') as f: | |
| data = yaml.load(f, Loader=yaml.FullLoader) | |
| self.diameter = data['diameter'] | |
| self.mass = mass | |
| self.weight = environment.g*mass | |
| self.area = pi*self.diameter**2/4.0 | |
| self.I_xy = mass*data['J_xy'] | |
| self.I_z = mass*data['J_z'] | |
| a = array(data['alpha']) | |
| cl = array(data['Cl']) | |
| cd = array(data['Cd']) | |
| cm = array(data['Cm']) | |
| self._alpha,self._Cl,self._Cd,self._Cm = self._flip(a,cl,cd,cm) | |
| kind = 'linear' | |
| self.Cl_func = interp1d(self._alpha, self._Cl, kind=kind) | |
| self.Cd_func = interp1d(self._alpha, self._Cd, kind=kind) | |
| self.Cm_func = interp1d(self._alpha, self._Cm, kind=kind) | |
| def _flip(self,a,cl,cd,cm): | |
| """ | |
| Data given from -90 deg to 90 deg. | |
| Expand to -180 to 180 using symmetry considerations. | |
| """ | |
| n = len(a) | |
| idx = argmin(abs(a)) | |
| a2 = zeros(2*n) | |
| cl2 = zeros(2*n) | |
| cd2 = zeros(2*n) | |
| cm2 = zeros(2*n) | |
| a2[idx:idx+n] = a[:] | |
| cl2[idx:idx+n] = cl[:] | |
| cd2[idx:idx+n] = cd[:] | |
| cm2[idx:idx+n] = cm[:] | |
| for i in range(idx): | |
| a2[i] = -(180 + a[idx-i]) | |
| cl2[i] = -cl[idx-i] | |
| cd2[i] = cd[idx-i] | |
| cm2[i] = -cm[idx-i] | |
| for i in range(idx+n,2*n): | |
| a2[i] = 180 - a[idx+n-i-2] | |
| cl2[i] = -cl[idx+n-i-2] | |
| cd2[i] = cd[idx+n-i-2] | |
| cm2[i] = -cm[idx+n-i-2] | |
| return a2,cl2,cd2,cm2 | |
| def _normalize_angle(self, alpha): | |
| """ | |
| Ensure that the angle fulfils :math:`-\\pi < \\alpha < \\pi` | |
| :param float alpha: Angle in radians | |
| :return: Normalized angle | |
| :rtype: float | |
| """ | |
| return arctan2(sin(alpha), cos(alpha)) | |
| def Cd(self, alpha): | |
| """ | |
| Provide drag coefficent for a given angle of attack. | |
| :param float alpha: Angle in radians | |
| :return: Drag coefficient | |
| :rtype: float | |
| """ | |
| # NB! The stored data uses degrees for the angle | |
| return self.Cd_func(degrees(self._normalize_angle(alpha))) | |
| def Cl(self, alpha): | |
| """ | |
| Provide drag coefficent for a given angle of attack. | |
| :param float alpha: Angle in radians | |
| :return: Drag coefficient | |
| :rtype: float | |
| """ | |
| # NB! The stored data uses degrees for the angle | |
| return self.Cl_func(degrees(self._normalize_angle(alpha))) | |
| def Cm(self, alpha): | |
| """ | |
| Provide coefficent of moment for a given angle of attack. | |
| :param float alpha: Angle in radians | |
| :return: Coefficient of moment | |
| :rtype: float | |
| """ | |
| # NB! The stored data uses degrees for the angle | |
| return self.Cm_func(degrees(self._normalize_angle(alpha))) | |
| def plot_coeffs(self, color='k'): | |
| """ | |
| Utility function to quickly explore disc coefficients. | |
| :param string color: Matplotlib color key. Default value is k, i.e. black. | |
| """ | |
| pl.plot(self._alpha, self._Cl, 'C0-o',label='$C_L$') | |
| pl.plot(self._alpha, self._Cd, 'C1-o',label='$C_D$') | |
| pl.plot(self._alpha, 3*self._Cm, 'C2-o',label='$C_M$') | |
| a = linspace(-pi,pi,200) | |
| #pl.plot(degrees(a), self.Cl(a), 'C0-',label='$C_L$') | |
| #pl.plot(degrees(a), self.Cd(a), 'C1-',label='$C_D$') | |
| #pl.plot(degrees(a), 3*self.Cm(a), 'C2-',label='$C_M$') | |
| pl.xlabel('Angle of attack ($^\circ$)') | |
| pl.ylabel('Aerodynamic coefficients (-)') | |
| pl.legend(loc='upper left') | |
| ax = pl.gca() | |
| ax2 = pl.gca().twinx() | |
| ax2.set_ylabel("Aerodynamic efficiency, $C_L/C_D$") | |
| pl.plot(self._alpha, self._Cl/self._Cd, 'C3-.',label='$C_L/C_D$') | |
| ax2.legend(loc='upper right') | |
| return ax,ax2 | |
| def empirical_spin(self, speed): | |
| # Simple empirical formula for spin rate, based on curve-fitting | |
| # data from: | |
| # https://www.dgcoursereview.com/dgr/forums/viewtopic.php?f=2&t=7097 | |
| #omega = -0.257*speed**2 + 15.338*speed | |
| # Alternatively, experiments indicate a linear relationship, | |
| omega = 5.2*speed | |
| return omega | |
| def initialize_shot(self, **kwargs): | |
| U = kwargs["speed"] | |
| kwargs.setdefault('yaw', 0.0) | |
| #kwargs.setdefault('omega', self.empirical_spin(U)) | |
| pitch = radians(kwargs["pitch"]) | |
| yaw = radians(kwargs["yaw"]) | |
| omega = kwargs["omega"] | |
| # phi, theta | |
| roll_angle = radians(kwargs["roll_angle"]) # phi | |
| nose_angle = radians(kwargs["nose_angle"]) # theta | |
| # psi, rotation around z irrelevant for starting position | |
| # since the disc is symmetric | |
| # Initialize position | |
| if "position" in kwargs: | |
| x,y,z = kwargs["position"] | |
| else: | |
| x = 0. | |
| y = 0. | |
| z = 0. | |
| # Initialize velocity | |
| xy = cos(pitch) | |
| u = U*xy*cos(yaw) | |
| v = U*xy*sin(-yaw) | |
| w = U*sin(pitch) | |
| # Initialize angles | |
| attitude = array([roll_angle, nose_angle, 0]) | |
| # The initial orientation of the disc must also account for the | |
| # angle of the throw itself, i.e. the launch angle. | |
| attitude += matmul(T_12(attitude), array((0, pitch, 0))) | |
| #attitude = matmul(T_23(yaw),attitude) | |
| #attitude += matmul(T_12(attitude), array((0, pitch, 0))) | |
| phi, theta, psi = attitude | |
| y0 = array((x,y,z,u,v,w,phi,theta,psi)) | |
| return y0, omega | |
| def shoot(self, **kwargs): | |
| y0, omega = self.initialize_shot(**kwargs) | |
| shot = self._shoot(self.advance, y0, omega) | |
| return shot | |
| def post_process(self, s, omega): | |
| n = len(s.time) | |
| alphas = zeros(n) | |
| betas = zeros(n) | |
| lifts = zeros(n) | |
| drags = zeros(n) | |
| moms = zeros(n) | |
| rolls = zeros(n) | |
| for i in range(n): | |
| x = s.position[:,i] | |
| u = s.velocity[:,i] | |
| a = s.attitude[:,i] | |
| alpha, beta, Fd, Fl, M, g4 = self.forces(x, u, a, omega) | |
| alphas[i] = alpha | |
| betas[i] = beta | |
| lifts[i] = Fl | |
| drags[i] = Fd | |
| moms[i] = M | |
| rolls[i] = -M/(omega*(self.I_xy - self.I_z)) | |
| arc_length = norm(s.position, axis=0) | |
| return arc_length,degrees(alphas),degrees(betas),lifts,drags,moms,degrees(rolls) | |
| def forces(self, x, u, a, omega): | |
| # Velocity in body axes | |
| urel = u - environment.wind_abl(x[2]) | |
| u2 = matmul(T_12(a), urel) | |
| # Side slip angle is the angle between the x and y velocity | |
| beta = -arctan2(u2[1], u2[0]) | |
| # Velocity in zero side slip axes | |
| u3 = matmul(T_23(beta), u2) | |
| # Angle of attack is the angle between | |
| # vertical and horizontal velocity | |
| alpha = -arctan2(u3[2], u3[0]) | |
| # Velocity in wind system, where forces are to be calculated | |
| u4 = matmul(T_34(alpha), u3) | |
| # Convert gravitational force from Earth to Wind axes | |
| g = array((0, 0, self.mass*environment.g)) | |
| g4 = T_14(g, a, beta, alpha) | |
| # Aerodynamic forces | |
| q = 0.5*environment.rho*u4[0]**2 | |
| S = self.area | |
| D = self.diameter | |
| Fd = q*S*self.Cd(alpha) | |
| Fl = q*S*self.Cl(alpha) | |
| M = q*S*D*self.Cm(alpha) | |
| return alpha, beta, Fd, Fl, M, g4 | |
| def advance(self, t, vec, omega): | |
| x = vec[0:3] | |
| u = vec[3:6] | |
| a = vec[6:9] | |
| alpha, beta, Fd, Fl, M, g4 = self.forces(x, u, a, omega) | |
| m = self.mass | |
| # Calculate accelerations | |
| dudt = (-Fd + g4[0])/m | |
| dvdt = g4[1]/m | |
| dwdt = ( Fl + g4[2])/m | |
| acc4 = array((dudt,dvdt,dwdt)) | |
| # Roll rate acts around x-axis (in axes 3: zero side slip axes) | |
| dphidt = -M/(omega*(self.I_xy - self.I_z)) | |
| # Other angular rotations are ignored, assume zero wobble | |
| angvel3 = array((dphidt, 0, 0)) | |
| acc1 = T_41(acc4, a, beta, alpha) | |
| angvel1 = T_31(angvel3, a, beta) | |
| return concatenate((u,acc1,angvel1)) | |