Configurable fin locations for torpedo vehicles

src/python_vehicle_simulator/lib/__init__.py

@@ -6,4 +6,5 @@
 from .plotTimeSeries import *
 from .guidance import *
 from .control import *
-from .models import *
+from .models import *
+from .fin import *

src/python_vehicle_simulator/lib/fin.py

@@ -0,0 +1,104 @@
+import numpy as np
+
+class fin:
+    '''
+    Represents a fin for hydrodynamic calculations.
+
+    INPUTS:
+        a:      fin area (m^2)
+        CL:     coefficient of lift (dimensionless)
+        x:      x distance (m) from center of vehicle (negative for behind COM)
+        c:      radius (m) from COP on the fin to the COM in the YZ plane
+        angle:  offset of fin angle around x axis (deg) starting from positive y
+                0 deg: fin on left side looking from front
+                90 deg: fin on bottom
+        cs:     control subsystem the fin is going to actuate for
+        rho:    density of fluid (kg/m^3) 
+
+    Coordinate system: Right-handed, x-forward, y-starboard, z-down
+    '''
+
+    def __init__(
+            self,
+            a,
+            CL,
+            x,
+            c = 0,
+            angle = 0,
+            rho = 1026,  # Default density of seawater
+    ):
+
+        self.area = a  # Fin area (m^2)
+        self.CL = CL   # Coefficient of lift (dimensionless)
+        self.angle_rad = np.deg2rad(angle)  # Convert angle to radians
+        self.rho = rho  # Fluid density (kg/m^3)
+
+        self.fin_actual = 0.0  #Actual position of the fin (rad)
+        self.T_delta = 0.1              # fin time constant (s) 
+        self.deltaMax = np.deg2rad(15) # max rudder angle (rad)
+
+        # Calculate fin's Center of Pressure (COP) position relative to COM
+        y = np.cos(self.angle_rad) * c  # y-component of COP (m)
+        z = np.sin(self.angle_rad) * c  # z-component of COP (m)
+        self.R = np.array([x, y, z])    # Location of COP of the fin relative to COM (m)
+
+    def velocity_in_rotated_plane(self, nu_r):
+        """
+        Calculate velocity magnitude in a plane rotated around the x-axis.
+
+        Parameters:
+            nu_r (numpy array): Velocity vector [vx, vy, vz] (m/s) in ENU frame
+
+        Returns:
+            float: Magnitude of velocity (m/s) in the rotated plane.
+        """
+        # Extract velocity components
+        vx, vy, vz = nu_r  # m/s
+
+        # Rotate y component around x-axis to align with fin plane
+        vy_rot = np.sqrt((vy * np.sin(self.angle_rad))**2 + (vz * np.cos(self.angle_rad))**2)
+
+        # Calculate magnitude in the rotated plane (x, y')
+        U_plane = np.sqrt(vx**2 + vy_rot**2)
+
+        return U_plane  # m/s
+
+    def torque(self, Ur):
+        """
+        Calculate torque generated by the fin.
+
+        Parameters:
+            Ur (numpy array): Relative velocity [vx, vy, vz, p, q, r] 
+                              (m/s for linear, rad/s for angular)
+            delta (float): Deflection angle of fin in radians
+                           (positive: CW rotation of fin and trailing edge) 
+
+        Returns:
+            numpy array: tau vector [Fx, Fy, Fz, Tx, Ty, Tz] (N*m) in body-fixed frame
+        """
+
+        ur = self.velocity_in_rotated_plane(Ur[:3])  # Calulate relative velocity in plane of the fin
+
+        # Calculate lift force magnitude
+        f = 0.5 * self.rho * self.area * self.CL * self.fin_actual * ur**2  # N
+
+        # Decompose force into y and z components
+        fy = np.sin(self.angle_rad) * f  # N
+        fz = -np.cos(self.angle_rad) * f  # N 
+
+        F = np.array([0, fy, fz])  # Force vector (N)
+
+        # Calculate torque using cross product of force and moment arm
+        torque = np.cross(self.R, F)  # N*m
+        return np.append(F, torque)
+
+    def actuate(self, sampleTime, command):
+        # Actuator dynamics        
+        delta_dot = (command - self.fin_actual) / self.T_delta  
+        self.fin_actual += sampleTime * delta_dot  # Euler integration 
+
+        # Amplitude Saturation
+        if abs(self.fin_actual) >= self.deltaMax:
+            self.fin_actual = np.sign(self.fin_actual) * self.deltaMax
+
+        return self.fin_actual

src/python_vehicle_simulator/lib/mainLoop.py

@@ -39,6 +39,7 @@ def printSimInfo():
     print('7 - Offshore supply vessel: controlled by tunnel thrusters and main propellers, L = 76.2 m')
     print('8 - Tanker: rudder-controlled ship model including shallow water effects, L = 304.8 m')
     print('9 - Remus 100: AUV controlled by stern planes, a tail rudder and a propeller, L = 1.6 m')
+    print('10 - Torpedo: AUV controlled by configurable fins and a propeller, L = 1.6 m')
     print('---------------------------------------------------------------------------------------')    
 
 ###############################################################################    

src/python_vehicle_simulator/main.py

@@ -41,6 +41,7 @@
 supply('DPcontrol',x_d,y_d,psi_d,V_c,beta_c)      
 tanker('headingAutopilot',psi_d,V_c,beta_c,depth)    
 remus100('depthHeadingAutopilot',z_d,psi_d,V_c,beta_c)             
+torpedo('depthHeadingAutopilot',z_d,psi_d,V_c,beta_c)             
 
 Call constructors without arguments to test step inputs, e.g. DSRV(), otter(), etc. 
 """
@@ -57,6 +58,7 @@
     case '7': vehicle = supply('DPcontrol',4.0,4.0,50.0,0.5,20.0)
     case '8': vehicle = tanker('headingAutopilot',-20,0.5,150,20,80)
     case '9': vehicle = remus100('depthHeadingAutopilot',30,50,1525,0.5,170)
+    case '10': vehicle = torpedo('depthHeadingAutopilot',30,50,1525,0.5,170)
     case _: print('Error: Not a valid simulator option'), sys.exit()
 
 printVehicleinfo(vehicle, sampleTime, N)

src/python_vehicle_simulator/vehicles/__init__.py

@@ -9,4 +9,5 @@
 from .shipClarke83 import *
 from .supply import *
 from .tanker import *
-from .remus100 import *
+from .remus100 import *
+from .torpedo import *