def obser(self, rudder_angle):
x = np.linspace(0, 1, num=100)
y0_1 = np.array([
self.state_0[0:4], self.state_0[4:8], self.state_0[8:12],
self.state_0[12:16]
]) # initial value
result_1 = odeintw(self.diff_equation1, y0_1, x, args=(rudder_angle, ))
#result_1 = odeintw(self.diff_equation1, y0_1, x)
#y0_2 = np.array([self.state_0[8:12], self.state_0[12:16]]) # initial value
#result_2 = odeintw(self.diff_equation2, y0_2, x, args = (rudder_angle,))
self.state_0 = np.vstack(
(result_1[-1, 0], result_1[-1, 1], result_1[-1, 2], result_1[-1,
3]))
print(self.state_0)
self.t += 1
self._t.append(round(self.t, 1))
self.x1.append(round(self.state_0.item(0), 1))
self.y1.append(round(self.state_0.item(1), 1))
self.z1.append(round(self.state_0.item(2), 1))
self.phi1.append(round(self.state_0.item(3), 1))
self.u1.append(round(self.state_0.item(4), 1))
self.v1.append(round(self.state_0.item(5), 1))
self.w1.append(round(self.state_0.item(6), 1))
self.r1.append(round(self.state_0.item(7), 1))
self.x2.append(round(self.state_0.item(8), 1))
self.y2.append(round(self.state_0.item(9), 1))
self.z2.append(round(self.state_0.item(10), 1))
self.phit.append(round(self.state_0.item(11), 1))
self.u2.append(round(self.state_0.item(12), 1))
self.v2.append(round(self.state_0.item(13), 1))
self.w2.append(round(self.state_0.item(14), 1))
self.r2.append(round(self.state_0.item(15), 1))
self.T.append(
round(Tether(self.state_0[0:4], self.state_0[8:12]).T(), 1))
self.Ffoil_x.append(
round(
Foil(self.state_0[8:12], self.state_0[12:16], self.c_dir,
self.c_speed).foilforce().item(0), 1))
self.Ffoil_z.append(
round(
Foil(self.state_0[8:12], self.state_0[12:16], self.c_dir,
self.c_speed).foilforce().item(2), 1))
#
self.Rudder_angle.append(round(rudder_angle, 1))
# self.Frudder_x.append(round(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(0),1))
# self.Frudder_y.append(round(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(1),1))
# self.Frudder_n.append(round(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(3),1))
data_storage(self.x1, self.y1, self.phit, self._t,
self.Rudder_angle) # store data in local files
observation = np.array([
self.state_0.item(8),
self.state_0.item(9),
self.state_0.item(11)
])
return observation
def obser(self, rudder_angle):
for _ in range(0, 10, 1):
# Runge-Kutta
k1 = self.f(self.state_0, rudder_angle) * self.time_step
k2 = self.f(self.state_0 + 0.5 * k1, rudder_angle) * self.time_step
k3 = self.f(
self.state_0 + 0.5 * k2,
rudder_angle,
) * self.time_step
k4 = self.f(self.state_0 + k3, rudder_angle) * self.time_step
#print(k2)
self.state_0 += (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
self.state_0[3] = self.change_angle(self.state_0.item(3))
#print(self.state_0.item(0))
self.t += 0.1
#print(self.state_0.item(12))
self._t.append(self.t)
self.x1.append(self.state_0.item(0))
self.y1.append(self.state_0.item(1))
self.z1.append(self.state_0.item(2))
self.phi1.append(self.state_0.item(3))
self.u1.append(self.state_0.item(4))
self.v1.append(self.state_0.item(5))
self.w1.append(self.state_0.item(6))
self.r1.append(self.state_0.item(7))
self.Rudder_angle.append(rudder_angle)
# self.Frudder_x.append(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(0))
# self.Frudder_y.append(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(1))
# self.Frudder_n.append(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(3))
data_storage(
self.x1,
self.y1,
self.phi1,
self.t,
u1=self.u1,
rudder_angle=self.Rudder_angle) # store data in local files
observation = np.array([
self.state_0.item(0),
self.state_0.item(1),
self.state_0.item(3), rudder_angle
])
return observation
def obser(self, rudder_angle):
for _ in range(0, 10, 1):
# Runge-Kutta
k1 = self.f(self.state_0, rudder_angle)* self.time_step
k2 = self.f(self.state_0 + 0.5 * k1, rudder_angle)* self.time_step
k3 = self.f(self.state_0 + 0.5 * k2, rudder_angle, )* self.time_step
k4 = self.f(self.state_0 + k3, rudder_angle)* self.time_step
self.state_0 += (1/6)*(k1 + 2 * k2 + 2 * k3 + k4)
self.state_0[3] = self.change_angle(self.state_0.item(3))
self.t += 0.1
self._t.append(self.t)
self.x1.append(self.state_0.item(0))
self.y1.append(self.state_0.item(1))
self.z1.append(self.state_0.item(2))
self.phi1.append(self.state_0.item(3))
self.u1.append(self.state_0.item(4))
self.v1.append(self.state_0.item(5))
self.w1.append(self.state_0.item(6))
self.r1.append(self.state_0.item(7))
self.Rudder_angle.append(rudder_angle)
data_storage(self.x1, self.y1, self.phi1, self.t, u1 = self.u1, rudder_angle = self.Rudder_angle) # store data in local files
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])
i_obstacle = self.distance_obstacle.index(min(self.distance_obstacle)) #
observation = np.array([self.distance_target, self.course_error_target, self.distance_obstacle[i_obstacle], self.course_error_obstacle[i_obstacle], rudder_angle])
return observation
def obser(self, rudder_angle):
for _ in range(0, 1000, 1):
# Runge-Kutta
k1 = self.WG.f(self.state_0, rudder_angle, self.t) * self.time_step
k2 = self.WG.f(self.state_0 + 0.5 * k1, rudder_angle,
self.t + 0.5 * self.time_step) * self.time_step
k3 = self.WG.f(self.state_0 + 0.5 * k2, rudder_angle,
self.t + 0.5 * self.time_step) * self.time_step
k4 = self.WG.f(self.state_0 + k3, rudder_angle,
self.t + self.time_step) * self.time_step
self.state_0 += (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
self.state_0[11] = self.change_angle(self.state_0.item(11))
self.state_0[3] = self.change_angle(self.state_0.item(3))
#print(self.state_0.item(3))
self.t += 0.001
#print(self.state_0.item(12))
self._t.append(self.t)
self.x1.append(self.state_0.item(0))
self.y1.append(self.state_0.item(1))
self.z1.append(self.state_0.item(2))
self.phi1.append(self.state_0.item(3))
print(self.state_0.item(3))
self.u1.append(self.state_0.item(4))
self.v1.append(self.state_0.item(5))
self.w1.append(self.state_0.item(6))
self.r1.append(self.state_0.item(7))
self.x2.append(self.state_0.item(8))
self.y2.append(self.state_0.item(9))
self.z2.append(self.state_0.item(10))
self.phit.append(self.state_0.item(11))
print(self.state_0.item(11))
self.u2.append(self.state_0.item(12))
self.v2.append(self.state_0.item(13))
self.w2.append(self.state_0.item(14))
self.r2.append(self.state_0.item(15))
self.T.append(Tether(self.state_0[0:4], self.state_0[8:12]).T())
print(Tether(self.state_0[0:4], self.state_0[8:12]).T())
self.Ffoil_x.append(
Foil(self.state_0[8:12], self.state_0[12:16], self.c_dir,
self.c_speed).foilforce().item(0))
self.Ffoil_z.append(
Foil(self.state_0[8:12], self.state_0[12:16], self.c_dir,
self.c_speed).foilforce().item(2))
self.Rudder_angle.append(rudder_angle)
# self.Frudder_x.append(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(0))
# self.Frudder_y.append(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(1))
# self.Frudder_n.append(Rudder(self.state_0[8:12], self.state_0[12:16], self.c_dir, self.c_speed).force(rudder_angle).item(3))
data_storage(
self.x2,
self.y2,
self.phit,
self.t,
u1=self.u2,
rudder_angle=self.Rudder_angle) # store data in local files
observation = np.array([
self.state_0.item(8),
self.state_0.item(9),
self.state_0.item(11)
])
return observation
def obser(self, rudder_angle):
k1 = self.f(self.state_0, rudder_angle)
k2 = self.f(self.state_0 + 0.5 * k1 * self.time_step, rudder_angle)
k3 = self.f(self.state_0 + 0.5 * k2 * self.time_step, rudder_angle)
k4 = self.f(self.state_0 + k3 * self.time_step, rudder_angle)
self.state_0 += (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4) * self.time_step
self.state_0[3] = self.change_angle(self.state_0.item(3))
self.t += 1
self._t.append(self.t)
self.x1.append(self.state_0.item(0))
self.y1.append(self.state_0.item(1))
self.z1.append(self.state_0.item(2))
self.phi1.append(self.state_0.item(3))
self.u1.append(self.state_0.item(4))
self.v1.append(self.state_0.item(5))
self.w1.append(self.state_0.item(6))
self.r1.append(self.state_0.item(7))
self.Rudder_angle.append(rudder_angle)
data_storage(
self.x1,
self.y1,
self.phi1,
self.t,
u1=self.u1,
rudder_angle=self.Rudder_angle) # store data in local files
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
])
i_obstacle = self.distance_obstacle.index(min(self.distance_obstacle))
eplison = 1
if self.distance_obstacle[i_obstacle] < 10:
eplison = -1
observation = np.array([
self.distance_target, self.course_error_target,
self.distance_obstacle[i_obstacle],
self.course_error_obstacle[i_obstacle]
])
if self.n_features == 5:
observation = np.array([
self.distance_target, self.course_error_target,
self.distance_obstacle[i_obstacle],
self.course_error_obstacle[i_obstacle], eplison
])
return observation
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