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