-
Notifications
You must be signed in to change notification settings - Fork 0
/
eom.py
123 lines (95 loc) · 2.93 KB
/
eom.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
import matplotlib.pyplot as plt
from scipy.integrate import odeint
import numpy as np
from collision import Collision
import math
class Obj():
def __init__(self):
self.d0 = 0
self.v0 = 0
self.d = 0
self.v = 0
self.a = 0
self.F = 0
self.t = 0
self.dt = 0.001
self.m = 0.1
self.B = 0.1
self.d_array = []
self.v_array = []
self.a_array = []
self.f_array = []
self.t_array = []
self.fext_array = []
self._m_store_trajectory = False
self._f_array_set = False
self._compute_collision = False
self.Col_obj = Collision()
self._sim_finished = False
def set_sim_finished(self, bool):
self._sim_finished = bool
def dynamics(self):
pass
def apply_force(self, F):
self.F = F
def set_force_array(self, force_array):
self.fext_array = force_array
self._f_array_set = True
def set_collision_check(self, bool):
self._compute_collision = bool
def update_force(self):
if self._f_array_set:
Fext1 = np.interp(self.t, self.fext_array.ft, self.fext_array.f_array[0])
if self._compute_collision:
Fext2 = self.Col_obj.compute_collision(self.d)
self.F = Fext1 - Fext2
def update(self, dt = 0.001):
self.d0 = self.d
self.v0 = self.v
if self._m_store_trajectory:
self.trajectory()
self.update_force()
self.dt = dt
self.t += self.dt
return self.eom()
def eom(self):
self.a = (self.F - self.B * self.v) / self.m
self.v = self.v0 + self.a * self.dt
self.d = self.d0 + (0.5 * (self.v0 + self.v) * self.dt)
return self.d, self.v
def set_mass(self, m):
self.m = m
def store_trajectory(self, check):
self._m_store_trajectory = check
def trajectory(self):
self.d_array.append(self.d0)
self.v_array.append(self.v0)
self.a_array.append(self.a)
self.f_array.append(self.F)
self.t_array.append(self.t)
def get_pos_last(self):
while self.d_array.__len__() > 0 and not self._sim_finished:
x = self.d_array[-1]
y = 0.0
yield x, y
def plot_trajectory(self, data_type='Position'):
plt.figure()
if data_type == 'Position':
Y = [self.d_array]
if data_type == 'Velocity':
Y = [self.v_array]
if data_type == 'Acceleration':
Y = [self.a_array]
if data_type == 'Force':
Y = [self.f_array]
if data_type == 'All':
Y = [self.d_array, self.v_array]
for i in range(0, len(Y)):
plt.plot(self.t_array, Y[i])
plt.xlabel('Time')
plt.ylabel(data_type)
plt.grid('on')
plt.show()
def plot_time(self):
plt.plot(self.t_array)
plt.show()