def reset(self):
time.sleep(0.1)
data_elimation() # Turn on when previous data needs to be cleared
self.t = 0
self._t.clear()
# float
self.x1.clear()
self.y1.clear()
self.z1.clear()
self.phi1.clear()
self.u1.clear()
self.v1.clear()
self.w1.clear()
self.r1.clear()
# glider
self.x2.clear()
self.y2.clear()
self.z2.clear()
self.phit.clear()
self.u2.clear()
self.v2.clear()
self.w2.clear()
self.r2.clear()
# forces
self.T.clear()
self.Ffoil_x.clear()
self.Ffoil_z.clear()
self.Rudder_angle.clear()
self.Frudder_x.clear()
self.Frudder_y.clear()
self.Frudder_n.clear()
# initial state
self.state_0 = np.array(
[
[0],
[0],
[0],
[0], # eta1
[0],
[0],
[0],
[0], # V1
[0],
[0],
[6.2],
[0], # eta2
[0],
[0],
[0],
[0]
],
float) # V2
#self.rudder_angle = [0]
return np.array([
self.state_0.item(8),
self.state_0.item(9),
self.state_0.item(11)
])
def reset(self):
time.sleep(0.1)
data_elimation() # Turn on when previous data needs to be cleared
self.t = 0
self._t.clear()
# float
self.x1.clear()
self.y1.clear()
self.z1.clear()
self.phi1.clear()
self.u1.clear()
self.v1.clear()
self.w1.clear()
self.r1.clear()
# forces
self.Thrust.clear()
self.Rudder_angle.clear()
self.Frudder_x.clear()
self.Frudder_y.clear()
self.Frudder_n.clear()
# initial state
self.state_0 = np.array([[0], [0], [0], [0], # eta1
[0], [0], [0], [0]], float) # V1
self.distance_target = math.hypot(self.state_0.item(0) - self.target_position[0],self.state_0.item(1) - self.target_position[1]) / 100
self.distance_obstacle_1 = (math.hypot(self.state_0.item(0) - self.obstacle_1[0],self.state_0.item(1) - self.obstacle_1[1])-self.obst_R1) / 100
self.distance_obstacle_2 = (math.hypot(self.state_0.item(0) - self.obstacle_2[0],self.state_0.item(1) - self.obstacle_2[1])-self.obst_R2) / 100
self.distance_obstacle_3 = (math.hypot(self.state_0.item(0) - self.obstacle_3[0],self.state_0.item(1) - self.obstacle_3[1])-self.obst_R3) / 100
self.distance_obstacle_4 = (math.hypot(self.state_0.item(0) - self.obstacle_4[0],self.state_0.item(1) - self.obstacle_4[1])-self.obst_R4) / 100
self.distance_obstacle_5 = (math.hypot(self.state_0.item(0) - self.obstacle_5[0],self.state_0.item(1) - self.obstacle_5[1])-self.obst_R5) / 100
self.course_error_target = self.state_0.item(3) - self.desired_course(self.target_position[0],self.target_position[1],self.state_0.item(0),self.state_0.item(1))
self.course_error_obstacle_1 = self.state_0.item(3) - self.desired_course(self.obstacle_1[0],self.obstacle_1[1],self.state_0.item(0),self.state_0.item(1))
self.course_error_obstacle_2 = self.state_0.item(3) - self.desired_course(self.obstacle_2[0],self.obstacle_2[1],self.state_0.item(0),self.state_0.item(1))
self.course_error_obstacle_3 = self.state_0.item(3) - self.desired_course(self.obstacle_3[0],self.obstacle_3[1],self.state_0.item(0),self.state_0.item(1))
self.course_error_obstacle_4 = self.state_0.item(3) - self.desired_course(self.obstacle_4[0],self.obstacle_4[1],self.state_0.item(0),self.state_0.item(1))
self.course_error_obstacle_5 = self.state_0.item(3) - self.desired_course(self.obstacle_5[0],self.obstacle_5[1],self.state_0.item(0),self.state_0.item(1))
self.distance_obstacle = list([self.distance_obstacle_1, self.distance_obstacle_2, self.distance_obstacle_3, self.distance_obstacle_4,self.distance_obstacle_5])
self.course_error_obstacle = list([self.course_error_obstacle_1, self.course_error_obstacle_2, self.course_error_obstacle_3,self.course_error_obstacle_4, self.course_error_obstacle_5])
# find the nearest obstacle
i_obstacle = self.distance_obstacle.index(min(self.distance_obstacle))
return np.array([self.distance_target, self.course_error_target, self.distance_obstacle[i_obstacle], self.course_error_obstacle[i_obstacle], 0])
def reset(self):
time.sleep(0.1)
data_elimation() # Turn on when previous data needs to be cleared
self.t = 0
self._t.clear()
# float
self.x1.clear()
self.y1.clear()
self.z1.clear()
self.phi1.clear()
self.u1.clear()
self.v1.clear()
self.w1.clear()
self.r1.clear()
# forces
self.Thrust.clear()
self.Rudder_angle.clear()
self.Frudder_x.clear()
self.Frudder_y.clear()
self.Frudder_n.clear()
# initial state
self.state_0 = np.array(
[
[0],
[0],
[0],
[0], # eta1
[0],
[0],
[0],
[0]
],
float) # V1
#self.rudder_angle = [0]
return np.array([
self.state_0.item(0),
self.state_0.item(1),
self.state_0.item(3), 0
])
def simulation():
# initial state
state_0 = np.array(
[
[0],
[0],
[0],
[0], # eta1
[0],
[0],
[0],
[0], # V1
[0],
[0],
[6.2],
[0], # eta2
[0],
[0],
[0],
[0]
],
float) # V2
# sea state
H = 1
omega = 0.15
c_dir = 0
c_speed = 0
WG = WG_dynamics(H, omega, c_dir, c_speed)
data_elimation() # Turn on when previous data needs to be cleared
for t in simulation_time(terminal_time=500):
rudder_angle = 0.5
# data storage
if t % 1 == 0:
_t.append(t)
x1.append(state_0.item(0))
y1.append(state_0.item(1))
z1.append(state_0.item(2))
phi1.append(state_0.item(3))
u1.append(state_0.item(4))
v1.append(state_0.item(5))
w1.append(state_0.item(6))
r1.append(state_0.item(7))
x2.append(state_0.item(8))
y2.append(state_0.item(9))
z2.append(state_0.item(10))
phit.append(state_0.item(11))
u2.append(state_0.item(12))
v2.append(state_0.item(13))
w2.append(state_0.item(14))
r2.append(state_0.item(15))
Rudder_angle.append(rudder_angle)
T0, Ffoil_0, Ffoil_2, Frudder_0, Frudder_1, Frudder_3 = WG.forces(
state_0, rudder_angle, t)
T.append(T0)
Ffoil_x.append(Ffoil_0)
Ffoil_z.append(Ffoil_2)
Frudder_x.append(Frudder_0)
Frudder_y.append(Frudder_1)
Frudder_n.append(Frudder_3)
# print(t)
data_storage(x1,
y1,
phit,
t,
rudder_angle=Rudder_angle,
u1=u1,
T=T) # store data in local files
# Runge-Kutta
time_step = 0.001
k1 = WG.f(state_0, rudder_angle, t)
k2 = WG.f(state_0 + 0.5 * k1 * time_step, rudder_angle,
t + 0.5 * time_step)
k3 = WG.f(state_0 + 0.5 * k2 * time_step, rudder_angle,
t + 0.5 * time_step)
k4 = WG.f(state_0 + k3 * time_step, rudder_angle, t + time_step)
state_0 += (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4) * time_step
'''
if state_0.item(11) > pi:
state_0[11] = state_0.item(11) - 2*pi
elif state_0.item(11) < -pi:
state_0[11] = state_0.item(11) + 2*pi
'''
# print(u1[-1], np.mean(u1))
# interval of refreshing the monitor, default: 2s
if t % 2 == 0:
data_viewer(x1,
y1,
u1,
phit,
Rudder_angle,
_t,
xlim_left=0,
xlim_right=200,
ylim_left=-100,
ylim_right=100,
goal_x=50,
goal_y=50) # T=T, Ffoil_x=Ffoil_x
return x1, y1, z1, phi1, \
x2, y2, z2, phit, \
u1, v1, w1, r1, \
u2, v2, w2, r2, \
_t, T, Ffoil_x, Ffoil_z, \
Frudder_x, Frudder_y, Frudder_n, Rudder_angle