-
Notifications
You must be signed in to change notification settings - Fork 0
/
Tether.py
163 lines (136 loc) · 6 KB
/
Tether.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
from Particle import Particle
import numpy as np
from unittest.mock import patch
import math
import copy
import scipy.constants
class Tether(Particle):
"""
"""
def __init__(self, Position=np.array([1,0,0], dtype=float), Velocity=np.array([0,0,0], dtype=float), name='Ball', mass=1.0, Theta=0.0, Length = 0., omega=np.array([0,0,0], dtype=float), alpha=np.array([0,0,0], dtype=float)):
super().__init__(Position=Position, Velocity=Velocity, name=name, mass=mass)
self.theta = self.set_theta()
self.length = self.set_length()
self.omega = self.set_omega()
self.alpha = alpha
def __repr__(self):
return '{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}'.format(self.name,self.mass,self.position, self.velocity,self.theta,self.length, self.omega, self.alpha)
g=9.81
def set_theta(self):
"""
Takes the input of cartesian coordinates in an array [x,y,z] and returns the polar coordiante angle theta
"""
if self.position[0] ==0:
if self.position[1] < 0:
return 0
else:
return scipy.constants.pi
if self.position[1] == 0:
if self.position[0] < 0:
return 3*scipy.constants.pi/2
else:
return scipy.constants.pi/2
# The above is for the 4 cases where the initial position begins on the x or y axis
else:
return np.arctan2(self.position[1],self.position[0]) + scipy.constants.pi/2
def set_length(self):
"""
Takes the input of cartesian coordinates in an array [x,y,z] and returns the length of the tether
"""
return np.linalg.norm(self.position)
def set_omega(self):
"""
Takes the cartesian position and initial linear velocity, and returns the angular velocity
"""
if self.length == 0: #prevents a divide by zero error
return [0, 0, 0]
else:
return (np.cross(self.position, self.velocity)/(self.length**2))
def update_alpha(self, deltaT):
"""
Takes the angular position theta and tether length from self, and also a time step deltaT
"""
if self.length == 0: #prevents a divide by zero error
return [0,0,0]
else:
self.alpha = np.array([0,0,-(9.81/self.length) * np.sin(self.theta)])
return self.alpha
def temp_alpha(self,deltaT,theta, omega):
"""
returns a new temporary angular acceleration vector for the Runge-Kutta method. It uses the given theta and omega values instead of the values from self
"""
return -(9.81/self.length) * np.sin(theta)
def update_omega(self, deltaT):
"""
Takes the angular acceleration vector and a time step deltaT and returns the updated angular velocity vector
"""
self.omega += self.alpha * deltaT
return self.omega
def update_theta(self, deltaT):
"""
Takes the angular velocity vector and a time step deltaT and return the scalar value of angular position
"""
self.theta += self.omega[2] * deltaT
return self.theta
def update_richardson(self, deltaT):
"""
Method for updating the position and angular velocity using the Euler-Richardson method
"""
omega_mid = self.omega +0.5*self.alpha*deltaT
theta_mid = self.theta +0.5*self.omega[2]*deltaT
alpha_mid = np.array([0,0,self.temp_alpha(deltaT, theta_mid, omega_mid)])
self.omega = self.omega + alpha_mid * deltaT
self.theta = self.theta + omega_mid[2] *deltaT
return self.theta, self.omega
def update_RK(self, deltaT):
"""
Method for updating the position and angular velocity using the Runge-Kutta method
"""
k1a, k1b, k2a, k2b, k3a, k3b, k4a, k4b = 0,0,0,0,0,0,0,0
k1a = deltaT * self.omega[2]
k1b = deltaT * self.temp_alpha(deltaT,self.theta,self.omega[2])
k2a = deltaT * (self.omega[2]+(k1b/2))
k2b = deltaT * self.temp_alpha(deltaT/2, (self.theta +(k1a/2)), (self.omega[2] + (k1b/2)))
k3a = deltaT * (self.omega[2]+(k2b/2))
k3b = deltaT * self.temp_alpha(deltaT/2, (self.theta +(k2a/2)), (self.omega[2] + (k2b/2)))
k4a = deltaT * (self.omega[2] + (k3b))
k4b = deltaT * self.temp_alpha(deltaT, (self.theta +(k3a)), (self.omega[2] + (k3b)))
theta_new = self.theta + ((k1a + 2*k2a + 2*k3a + k4a)/6)
omega_new =np.array([0,0, self.omega[2] + ((k1b + 2*k2b + 2*k3b + k4b)/6)])
return theta_new, omega_new
def update_angular(self, deltaT, method):
"""
The main function which dictates which update method is used.
"""
if method == "E": #Euler method
self.update_alpha(deltaT)
self.update_theta(deltaT)
self.update_omega(deltaT)
elif method == "EC": #Euler-Cromer method
self.update_alpha(deltaT)
self.update_omega(deltaT)
self.update_theta(deltaT)
elif method == "ER": #Euler-Richardson method
self.update_alpha(deltaT)
self.update_richardson(deltaT)
elif method == "RK": #Runge-Kutta method
self.theta, self.omega = self.update_RK(deltaT)
return self
def update_position(self):
"""
Converts from polar position to cartesian position
"""
self.position[0] = self.length * np.cos(self.theta - scipy.constants.pi/2)
self.position[1] = self.length * np.sin(self.theta - scipy.constants.pi/2)
return self.position
def update_velocity(self):
"""
Converts from angular velocity to linear velocity
"""
self.velocity = np.cross(self.omega, self.position)
return self.velocity
def KE_angular(self):
"""
Calculates the kinetic energy from the angular velocity
"""
return 0.5 * self.mass * (self.length**2) *np.vdot(self.omega, self.omega)