-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathphysics_sim.py
172 lines (141 loc) · 6.91 KB
/
physics_sim.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
164
165
166
167
168
169
170
171
172
import numpy as np
import csv
def C(x):
return np.cos(x)
def S(x):
return np.sin(x)
def earth_to_body_frame(ii, jj, kk):
# C^b_n
R = [[C(kk) * C(jj), C(kk) * S(jj) * S(ii) - S(kk) * C(ii), C(kk) * S(jj) * C(ii) + S(kk) * S(ii)],
[S(kk) * C(jj), S(kk) * S(jj) * S(ii) + C(kk) * C(ii), S(kk) * S(jj) * C(ii) - C(kk) * S(ii)],
[-S(jj), C(jj) * S(ii), C(jj) * C(ii)]]
return np.array(R)
def body_to_earth_frame(ii, jj, kk):
# C^n_b
return np.transpose(earth_to_body_frame(ii, jj, kk))
class PhysicsSim():
def __init__(self, init_pose=None, init_velocities=None, init_angle_velocities=None, runtime=5.):
self.init_pose = init_pose
self.init_velocities = init_velocities
self.init_angle_velocities = init_angle_velocities
self.runtime = runtime
self.gravity = -9.81 # m/s
self.rho = 1.2
self.mass = 0.958 # 300 g
self.dt = 1 / 50.0 # Timestep
self.C_d = 0.3
self.l_to_rotor = 0.4
self.propeller_size = 0.1
width, length, height = .51, .51, .235
self.dims = np.array([width, length, height]) # x, y, z dimensions of quadcopter
self.areas = np.array([length * height, width * height, width * length])
# CORRECTED INERTIA FORMULA for I_x and I_y
I_x = 1 / 12. * self.mass * (length**2 + height**2)
I_y = 1 / 12. * self.mass * (height**2 + width**2 ) # 0.0112 was a measured value
I_z = 1 / 12. * self.mass * (width**2 + length**2)
self.moments_of_inertia = np.array([I_x, I_y, I_z]) # moments of inertia
self.T_q = 0.1 # Thrust to torque ratio. Probably not actually linear... But, let's just go with it.
env_bounds = 300.0 # 300 m / 300 m / 300 m
self.lower_bounds = np.array([-env_bounds / 2, -env_bounds / 2, 0])
self.upper_bounds = np.array([env_bounds / 2, env_bounds / 2, env_bounds])
self.reset()
def reset(self):
self.time = 0.0
self.pose = np.array([0.0, 0.0, 10.0, 0.0, 0.0, 0.0]) \
if self.init_pose is None else np.copy(self.init_pose)
self.v = np.array([0.0, 0.0, 0.0]) \
if self.init_velocities is None else np.copy(self.init_velocities)
self.angular_v = np.array([0.0, 0.0, 0.0]) \
if self.init_angle_velocities is None else np.copy(self.init_angle_velocities)
self.linear_accel = np.array([0.0, 0.0, 0.0])
self.angular_accels = np.array([0.0, 0.0, 0.0])
self.prop_wind_speed = np.array([0., 0., 0., 0.])
self.done = False
def find_body_velocity(self):
body_velocity = np.matmul(earth_to_body_frame(*list(self.pose[3:])), self.v)
return body_velocity
def get_linear_drag(self):
linear_drag = 0.5 * self.rho * self.find_body_velocity()**2 * self.areas * self.C_d
return linear_drag
'''
# Fix by https://discussions.udacity.com/t/no-progress-on-quadcopter-hover-task-or-mountain-car/665833/4
linear_drag = 0.5 * self.rho * self.find_body_velocity()**2 * self.areas * self.C_d
for i in range(3):
if (self.v[i] > 0):
linear_drag[i] *= (-1)
return linear_drag
'''
def get_linear_forces(self, thrusts):
# Gravity
gravity_force = self.mass * self.gravity * np.array([0, 0, 1])
# Thrust
thrust_body_force = np.array([0, 0, sum(thrusts)])
# Drag
'''
# Fix by https://discussions.udacity.com/t/no-progress-on-quadcopter-hover-task-or-mountain-car/665833/4
drag_body_force = self.get_linear_drag()
'''
drag_body_force = -self.get_linear_drag()
body_forces = thrust_body_force + drag_body_force
linear_forces = np.matmul(body_to_earth_frame(*list(self.pose[3:])), body_forces)
linear_forces += gravity_force
return linear_forces
def get_moments(self, thrusts):
thrust_moment = np.array([(thrusts[0] + thrusts[3] - thrusts[1] - thrusts[2]) * self.l_to_rotor,
(thrusts[2] + thrusts[3] - thrusts[0] - thrusts[1]) * self.l_to_rotor,
(thrusts[0] + thrusts[2] - thrusts[1] - thrusts[3]) * self.T_q])
drag_moment = self.C_d * 0.5 * self.rho * self.angular_v * \
np.absolute(self.angular_v) * self.areas * self.dims * self.dims
moments = thrust_moment - drag_moment # + motor_inertia_moment
return moments
def calc_prop_wind_speed(self):
body_velocity = self.find_body_velocity()
phi_dot, theta_dot = self.angular_v[0], self.angular_v[1]
s_0 = np.array([0., 0., theta_dot * self.l_to_rotor])
s_1 = -s_0
s_2 = np.array([0., 0., phi_dot * self.l_to_rotor])
s_3 = -s_2
speeds = [s_0, s_1, s_2, s_3]
for num in range(4):
perpendicular_speed = speeds[num] + body_velocity
self.prop_wind_speed[num] = perpendicular_speed[2]
def get_propeler_thrust(self, rotor_speeds):
'''calculates net thrust (thrust - drag) based on velocity
of propeller and incoming power'''
thrusts = []
for prop_number in range(4):
V = self.prop_wind_speed[prop_number]
D = self.propeller_size
n = rotor_speeds[prop_number]
J = V / (n * D)
# From http://m-selig.ae.illinois.edu/pubs/BrandtSelig-2011-AIAA-2011-1255-LRN-Propellers.pdf
C_T = max(.12 - .07*max(0, J)-.1*max(0, J)**2, 0)
thrusts.append(C_T * self.rho * n**2 * D**4)
return thrusts
def next_timestep(self, rotor_speeds):
self.calc_prop_wind_speed()
thrusts = self.get_propeler_thrust(rotor_speeds)
self.linear_accel = self.get_linear_forces(thrusts) / self.mass
position = self.pose[:3] + self.v * self.dt + 0.5 * self.linear_accel * self.dt**2
self.v += self.linear_accel * self.dt
moments = self.get_moments(thrusts)
self.angular_accels = moments / self.moments_of_inertia
angles = self.pose[3:] + self.angular_v * self.dt + 0.5 * self.angular_accels * self.dt ** 2
# CORRECTED THE EQUATION FOR angles
angles = (angles + 2 * np.pi) % (2 * np.pi)
self.angular_v = self.angular_v + self.angular_accels * self.dt
new_positions = []
for ii in range(3):
if position[ii] <= self.lower_bounds[ii]:
new_positions.append(self.lower_bounds[ii])
self.done = True
elif position[ii] > self.upper_bounds[ii]:
new_positions.append(self.upper_bounds[ii])
self.done = True
else:
new_positions.append(position[ii])
self.pose = np.array(new_positions + list(angles))
self.time += self.dt
if self.time > self.runtime:
self.done = True
return self.done