1#
2#     This file is part of CasADi.
3#
4#     CasADi -- A symbolic framework for dynamic optimization.
5#     Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
6#                             K.U. Leuven. All rights reserved.
7#     Copyright (C) 2011-2014 Greg Horn
8#
9#     CasADi is free software; you can redistribute it and/or
10#     modify it under the terms of the GNU Lesser General Public
11#     License as published by the Free Software Foundation; either
12#     version 3 of the License, or (at your option) any later version.
13#
14#     CasADi is distributed in the hope that it will be useful,
15#     but WITHOUT ANY WARRANTY; without even the implied warranty of
16#     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17#     Lesser General Public License for more details.
18#
19#     You should have received a copy of the GNU Lesser General Public
20#     License along with CasADi; if not, write to the Free Software
21#     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
22#
23#
24# -*- coding: utf-8 -*-
25from casadi import *
26import matplotlib.pyplot as plt
27import numpy
28
29# Sailboat model based on
30#
31# [MF2011]:
32# Y. Masuyama, Toichi Fukasawa
33# "Tacking Simulation of Sailing Yachts with New Model of Aerodynamic
34# Force Variation During Tacking Maneuver"
35# Journal of Sailboat Technology, Article 2011-01
36#
37# Joel Andersson, UW Madison 2017
38#
39
40# Create DaeBuilder instance
41dae = DaeBuilder()
42
43# Load file with external functions
44from os import path
45curr_dir = path.dirname(path.abspath(__file__))
46clib = Importer(curr_dir + '/sailboat_c.c', 'none')
47
48# Options for external functions
49# NOTE: These options should become redundant once code is more stable
50external_opts = dict(enable_jacobian = False, enable_forward = False, \
51                     enable_reverse = False, enable_fd = True)
52
53# Physical constants
54g = 9.81 # [m/s^2] gravity
55rho = 1027. # p[kg/m^3] density of ocean water
56
57# Sailboat model constants. Cf. Table 1 [MF2011]
58L = 8.80 # [m] Length of design waterline
59D = 2.02 # [m] Design draft, including fin keel
60m = 4410. # [kg] Displacement
61GM = 1.45 # [m] metacentric height of boat
62m_x = 160.; m_y_hull = 2130.; m_y_sail = 280.; m_z = 12000. # [kg] Added masses
63Ixx = 17700.; Iyy = 33100.; Izz = 17200. # [kg m^2] Moments of inertia
64Jxx_hull = 7200.; Jxx_sail = 8100.; Jyy = 42400.; Jzz = 6700. # [kg m^2] Added moments of inertia
65X_pVV = 3.38e-1
66X_pPP = 1.40e-3
67X_pVVVV = -1.84
68X_pT = -1.91e-2
69Y_pV = -5.35e-1
70Y_pP = -5.89e-3
71Y_pVPP = 7.37e-1
72Y_pVVP = -5.53e-1
73Y_pVVV = 3.07
74Y_pP = 2.19e-1
75Y_pT = -4.01e-3
76K_pV = 2.80e-1
77K_pP = 3.36e-3
78K_pVPP = -4.07e-1
79K_pVVP = 2.24e-1
80K_pVVV = -1.38
81K_pT = -3.53e-1
82N_pV = -3.23e-2
83N_pP = -1.52e-2
84N_pVPP = 2.71e-4
85N_pVVP = -9.06e-2
86N_pVVV = -2.98e-2
87N_pT = -5.89e-3
88C_Xd = -3.79e-2
89C_Yd = -1.80e-1
90C_Kd = 9.76e-2
91C_Nd = 9.74e-2
92
93# States
94U = dae.add_x('U') # Velocity along the X axis
95V = dae.add_x('V') # Velocity along the Y axis
96phi = dae.add_x('phi') # Roll angle
97theta = dae.add_x('theta') # Yaw angle
98dphi = dae.add_x('dphi') # Time derivative of phi
99dtheta = dae.add_x('dtheta') # Time derivative of theta
100
101# Controls
102beta = dae.add_u('beta')
103
104# Sum contributions from hull and sail (?)
105m_y = m_y_hull + m_y_sail
106Jxx = Jxx_hull + Jxx_sail
107
108# Auxiliary variables
109
110# Squared boat velocity
111V_B2 = U**2 + V**2
112
113# To avoid duplicate expressions
114cos_phi = cos(phi)
115sin_phi = sin(phi)
116cos2_phi = cos_phi**2
117sin2_phi = sin_phi**2
118phi2 = phi**2
119
120# Hull resistance in the upright position
121X_0_fun = dae.add_fun('hull_resistance', clib, external_opts)
122X_0 = X_0_fun(U)
123
124# Calculate hydrodynamic forces
125V_p = sin(beta)
126V_p2 = V_p**2
127V_p3 = V_p2*V_p
128V_p4 = V_p2**2
129H_fact = 0.5*rho*V_B2*L*D
130dae.add_d('X_H', (X_pVV*V_p2 + X_pPP*phi2 + X_pVVVV*V_p4)*H_fact)
131dae.add_d('Y_H', (Y_pV*V_p + Y_pP*phi + Y_pVPP*V_p*phi2 + Y_pVVP*V_p2*phi + Y_pVVV*V_p3)*H_fact)
132dae.add_d('K_H', (K_pV*V_p + K_pP*phi + K_pVPP*V_p*phi2 + K_pVVP*V_p2*phi + K_pVVV*V_p3)*H_fact*D)
133dae.add_d('N_H', (N_pV*V_p + N_pP*phi + N_pVPP*V_p*phi2 + N_pVVP*V_p2*phi + N_pVVV*V_p3)*H_fact*L)
134H = dae.add_fun('H', ['phi', 'beta', 'U', 'V'], ['X_H', 'Y_H', 'K_H', 'N_H'])
135
136# Plot it for reference
137ngrid_phi = 100; ngrid_beta = 100
138U0 = 5.; V0 = 5. # [m/s] Boat speed for simulation
139phi0 = numpy.linspace(-pi/4, pi/4, ngrid_phi)
140beta0 = numpy.linspace(-pi/4, pi/4, ngrid_beta)
141PHI0,BETA0 = numpy.meshgrid(phi0, beta0)
142r = H(phi=PHI0, beta=BETA0, U=U0, V=V0)
143for i,c in enumerate(['X_H', 'Y_H', 'K_H', 'N_H']):
144    plt.subplot(2,2,i+1)
145    CS = plt.contour(PHI0*180/pi, BETA0*180/pi, log10(r[c]))
146    plt.clabel(CS, inline=1, fontsize=10)
147    plt.title('log10(' + c + ')')
148    plt.grid(True)
149
150# Make a function call
151X_H, Y_H, K_H, N_H = H(phi, beta, U, V)
152
153# Hydrodynamic derivatives of the hull due to yawing motion
154X_VT = 0. # Neglected
155Y_T = 0. # Neglected
156N_T = 0. # Neglected
157
158# Derivative due to rolling
159Y_P = 0. # Neglected
160K_P = 0. # Neglected
161
162# Hydrodynamic forces on the rudder
163X_R = 0. # Neglected
164Y_R = 0. # Neglected
165K_R = 0. # Neglected
166N_R = 0. # Neglected
167
168# Sail forces
169X_S = 0. # Neglected
170Y_S = 0. # Neglected
171K_S = 0. # Neglected
172N_S = 0. # Neglected
173
174# Surge: (m+m_x)*dot(U) = F_X, cf. (3) [MF2011]
175F_X = X_0 + X_H + X_R + X_S \
176    + (m + m_y*cos2_phi + m_z*sin2_phi + X_VT)*V*dtheta
177dae.add_ode("surge", F_X / (m+m_x))
178
179# Sway: (m + m_y*cos2_phi + m_z*sin2_phi)*dot(V) = F_Y
180F_Y = Y_H + Y_P*dphi + Y_T*dtheta + Y_R + Y_S \
181    - (m + m_x)*U*dtheta \
182    - 2*(m_z - m_y)*sin_phi*cos_phi*V*dphi
183dae.add_ode("sway", F_Y / (m + m_y*cos2_phi + m_z*sin2_phi))
184
185# Roll: (Ixx + Jxx)*dot(dphi) = F_K
186F_K = K_H + K_P*dphi + K_R + K_S - m*g*GM*sin_phi \
187    + ((Iyy+Jyy)-(Izz+Jzz))*sin_phi*cos_phi*dtheta**2
188dae.add_ode("roll", F_K / (Ixx + Jxx))
189
190# Yaw: ((Iyy+Jyy)*sin2_phi + (Izz+Jzz)*cos2_phi)*dot(dtheta) = F_N
191F_N = N_H + N_T*dtheta + N_R + N_S \
192    -2*((Iyy+Jyy)-(Izz+Jzz))*sin_phi*cos_phi*dtheta*dphi
193dae.add_ode("yaw", F_N / ((Iyy+Jyy)*sin2_phi + (Izz+Jzz)*cos2_phi))
194
195# Roll angle
196dae.add_ode("roll_angle", dphi)
197
198# Yaw angle
199dae.add_ode("yaw_angle", dtheta)
200
201# Print ODE
202print(dae)
203
204# Generate Jacobian of ODE rhs w.r.t. to states and control
205Jfcn = dae.create("Jfcn", ['x', 'u'], ['jac_ode_x', 'jac_ode_u'])
206Jfcn_file = Jfcn.generate()
207print('Jacobian function saved to ' + Jfcn_file)
208
209plt.show()
210