def __init__(self, eta1, eta2, V1, V2, c_dir, c_speed):
# states
V1_r = V1 - Vc(c_dir, c_speed, eta1)
V2_r = V2 - Vc(c_dir, c_speed, eta2)
self.u1_r = V1_r.item(0)
self.v1_r = V1_r.item(1)
self.r1 = V1_r.item(3)
self.u2_r = V2_r.item(0)
self.v2_r = V2_r.item(1)
self.r2 = V2_r.item(3)
# parameters
self.rho = 1025
self.L = np.array([[0, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0],
[0, 0, 0, 0]])
def __init__(self, eta2, V2, c_dir, c_speed):
self.rho = 1025
self.A = 0.113
self.lamda = 2
V2_r = V2 - Vc(c_dir, c_speed, eta2)
self.u2_r = V2_r.item(0)
self.w2_r = V2_r.item(2)
self.vf = sqrt(self.u2_r**2 + self.w2_r**2)
self.alpha_f = atan2(abs(self.w2_r), abs(self.u2_r))
def f(self, state, angle, t):
# float's position and attitude vector
eta1 = state[0:4]
#eta1[2] = self.H / 2 * sin(self.omega * t)
WF = np.array([[0], [0], [5000 * self.H/2*sin(self.omega*t)], [0]])
# float's velocity vector
V1 = state[4:8]
# glider's position and attitude vector
eta2 = state[8:12]
# glider's velocity vector
V2 = state[12:16]
# float's relative velocity vector
V1_r = V1 - Vc(self.c_dir, self.c_speed, eta1)
# glider's relative velocity vector
V2_r = V2 - Vc(self.c_dir, self.c_speed, eta2)
wg = WG(eta1, eta2, V1, V2, self.c_dir, self.c_speed)
tether = Tether(eta1, eta2)
foil = Foil(eta2, V2, self.c_dir, self.c_speed)
rudder = Rudder(eta2, V2, self.c_dir, self.c_speed)
# float's kinematic equations
eta1_dot = np.dot(J(eta1), V1)
# glider's kinematic equations
eta2_dot = np.dot(J(eta2), V2)
Minv_1 = np.linalg.inv(wg.MRB_1() + wg.MA_1())
Minv_2 = np.linalg.inv(wg.MRB_2() + wg.MA_2())
#gravaty and float limitation
if -0.2 < eta1[2] < 0.1:
Fgravity = np.dot(wg.G_1(), eta1)
elif eta1[2] < -0.2:
Fgravity = np.array([[0],[0],[-2200],[0]])
else:
Fgravity = np.array([[0],[0],[1100],[0]])
MV1_dot = - np.dot(wg.CRB_1(), V1) - np.dot(wg.CA_1(), V1_r) - np.dot(wg.D_1(), V1_r) - wg.d_1() - Fgravity + tether.Ftether_1() + WF
MV2_dot = - np.dot(wg.CRB_2(), V2) - np.dot(wg.CA_2(), V2_r) - wg.d_2() - wg.g_2() + tether.Ftether_2() + rudder.force(angle) + foil.foilforce()
# float's dynamic equations
V1_dot = np.dot(Minv_1, MV1_dot)
# glider's dynamic equations
V2_dot = np.dot(Minv_2, MV2_dot)
return np.vstack((eta1_dot, V1_dot, eta2_dot, V2_dot))
def diff_equation1(self, y_list, t, rudder_angle):
eta1, V1, eta2, V2 = y_list
#eta1[2] = self.H / 2 * sin(self.omega * self.t)
WF = np.array([[0], [0],
[5000 * self.H / 2 * sin(self.omega * self.t)], [0]])
V1_r = V1 - Vc(self.c_dir, self.c_speed, eta1)
V2_r = V2 - Vc(self.c_dir, self.c_speed, eta2)
wg = WG(eta1, eta2, V1, V2, self.c_dir, self.c_speed)
tether = Tether(eta1, eta2)
foil = Foil(eta2, V2, self.c_dir, self.c_speed)
rudder = Rudder(eta2, V2, self.c_dir, self.c_speed)
# float's kinematic equations
eta1_dot = np.dot(J(eta1), V1)
# glider's kinematic equations
eta2_dot = np.dot(J(eta2), V2)
Minv_1 = np.linalg.inv(wg.MRB_1() + wg.MA_1())
Minv_2 = np.linalg.inv(wg.MRB_2() + wg.MA_2())
# gravity and float limitation
if -0.2 < eta1[2] < 0.1:
Fgravity = np.dot(wg.G_1(), eta1)
elif eta1[2] < -0.2:
Fgravity = np.array([[0], [0], [-2200], [0]])
else:
Fgravity = np.array([[0], [0], [1100], [0]])
MV1_dot = -np.dot(wg.CRB_1(), V1) - np.dot(wg.CA_1(), V1_r) - np.dot(
wg.D_1(), V1_r) + wg.d_1() - Fgravity + tether.Ftether_1() + WF
MV2_dot = -np.dot(wg.CRB_2(), V2) - np.dot(
wg.CA_2(), V2_r) - wg.d_2() - wg.g_2() + tether.Ftether_2(
) + rudder.force(rudder_angle) + foil.foilforce()
# float's dynamic equations
V1_dot = np.dot(Minv_1, MV1_dot)
# glider's dynamic equations
V2_dot = np.dot(Minv_2, MV2_dot)
return np.array([eta1_dot, V1_dot, eta2_dot, V2_dot])
def force(self, angle):
# states
V2_r = self.V2 - Vc(self.c_dir, self.c_speed, self.eta2)
ur_2 = V2_r.item(0)
# parameters
LCG = 0.94
lamda = 2
chi = 7 * pi / 180
A = 0.04
CDC = 0.8
CD0 = 0.008
rho = 1025
# lift and drag forces
m = sqrt(4 + lamda ** 2 / cos(chi) ** 4)
CL = 1.8 * pi * lamda * angle / (1.8 + cos(chi) * m) + CDC * angle ** 2 / lamda
CD = CD0 + CL ** 2 / 0.9 / lamda / pi
FL = 0.5 * rho * CL * A * ur_2 ** 2
#print(FL)
FD = 0.5 * rho * CD * A * ur_2 ** 2
#print(FD)
return np.array([[-FD], [FL], [0], [- FL * LCG]])
def f(self, state, angle):
# float's position and attitude vector
eta1 = state[0:4]
#eta1[2] = self.H / 2 * sin(self.omega * t)
WF = np.array([[20], [0], [0], [0]])
# float's velocity vector
V1 = state[4:8]
# float's relative velocity vector
V1_r = V1 - Vc(self.c_dir, self.c_speed, eta1)
wg = WG(eta1, eta1, V1, V1, self.c_dir, self.c_speed)
rudder = Rudder(eta1, V1, self.c_dir, self.c_speed)
# float's kinematic equations
eta1_dot = np.dot(J(eta1), V1)
Minv_1 = np.linalg.inv(wg.MRB_1() + wg.MA_1())
MV1_dot = - np.dot(wg.CRB_1(), V1) - np.dot(wg.CA_1(), V1_r) - np.dot(wg.D_1(), V1_r) - wg.d_1() + rudder.force(angle) + WF
V1_dot = np.dot(Minv_1, MV1_dot)
return np.vstack((eta1_dot, V1_dot))