FEAT: Working fin class for configurable fins. Added some comments

Author: BradenMeyers

src/python_vehicle_simulator/lib/fin.py

@@ -18,24 +18,19 @@ class fin:
     Coordinate system: Right-handed, x-forward, y-starboard, z-down
     '''
 
-
-    ######################### TODO add actuator dynamics
-    # - saturating 
     def __init__(
             self,
             a,
             CL,
             x,
             c = 0,
             angle = 0,
-            control_subsystem='heading',
             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.control = control_subsystem
         self.rho = rho  # Fluid density (kg/m^3)
 
         self.fin_actual = 0.0  #Actual position of the fin (rad)
@@ -82,7 +77,7 @@ def torque(self, Ur):
             numpy array: tau vector [Fx, Fy, Fz, Tx, Ty, Tz] (N*m) in body-fixed frame
         """
 
-        ur = self.velocity_in_rotated_plane(Ur[:3])  # Use only linear velocities
+        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
@@ -92,18 +87,17 @@ def torque(self, Ur):
         fz = -np.cos(self.angle_rad) * f  # N 
 
         F = np.array([0, fy, fz])  # Force vector (N)
-        # print("Force", F)
-        # print("Radius:", self.R)
 
         # 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):
-        
-        delta_dot = (command - self.fin_actual) / self.T_delta
-        self.fin_actual += sampleTime * delta_dot
+        # 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
 

src/python_vehicle_simulator/vehicles/torpedo.py

@@ -17,15 +17,18 @@
         psi_d:  desired yaw angle (deg)
         n_d:    desired propeller revolution (rpm)
         V_c:    current speed (m/s)
-        beta_c: current direction (deg)                  
+        beta_c: current direction (deg)
+        fins:   number of fins (equally spaced)                  
 
 Methods:
         
     [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime ) returns 
         nu[k+1] and u_actual[k+1] using Euler's method. The control input is:
 
-            u_control = [ delta_r   rudder angle (rad)
-                         delta_s    stern plane angle (rad)
+            u_control = [ delta_r_top   rudder angle (rad)
+                         delta_r_bottom rudder angle (rad)
+                         delta_s_star    stern plane angle (rad)
+                         delta_s_port    stern plane angle (rad)
                          n          propeller revolution (rpm) ]
 
     u = depthHeadingAutopilot(eta,nu,sampleTime) 
@@ -115,7 +118,6 @@ def __init__(
 
         self.nu = np.array([0, 0, 0, 0, 0, 0], float) # velocity vector
         self.u_actual = np.zeros(fins+1, float)    # control input vector
-        # TODO make this the length of the fins
         self.controls = [
             "T Tail rudder (deg)",
             "B Tail rudder (deg)",
@@ -189,14 +191,14 @@ def __init__(
         self.w_pitch = math.sqrt( self.W * ( self.r_bg[2]-self.r_bb[2] ) / 
             self.M[4][4] )
 
-        S_fin = 0.00665;            # one fin area 
+        S_fin = 0.00665;            # fin area 
         CL_delta_r = 0.5            # rudder lift coefficient
         CL_delta_s = 0.7            # stern-plane lift coefficient
 
-        portSternFin = fin(S_fin, CL_delta_s, -a, angle=0)       
-        bottomRudderFin = fin(S_fin, CL_delta_r, -a, angle=90)  
-        starSternFin = fin(S_fin, CL_delta_s, -a, angle=180)      
-        topRudderFin = fin(S_fin, CL_delta_r, -a, angle=270)  
+        portSternFin = fin(S_fin, CL_delta_s, -a, angle=0, rho=self.rho)       
+        bottomRudderFin = fin(S_fin, CL_delta_r, -a, angle=90, rho=self.rho)  
+        starSternFin = fin(S_fin, CL_delta_s, -a, angle=180, rho=self.rho)      
+        topRudderFin = fin(S_fin, CL_delta_r, -a, angle=270, rho=self.rho)  
         self.fins = [topRudderFin, bottomRudderFin , starSternFin, portSternFin]
 
         # Low-speed linear damping matrix parameters
@@ -344,17 +346,16 @@ def dynamics(self, eta, nu, u_actual, u_control, sampleTime):
         # Restoring forces and moments
         g = gvect(self.W,self.B,eta[4],eta[3],self.r_bg,self.r_bb)
 
-        # Fin torques
+        # Fin force vector
         tau_fins = np.zeros(6,float)
         for i in range(len(self.fins)):
             tau_fins += self.fins[i].torque(nu_r)
-            u_actual[i] = self.fins[i].actuate(sampleTime, u_control[i])
+            u_actual[i] = self.fins[i].actuate(sampleTime, u_control[i]) # Actuator Dynamics
 
-        # Generalized force vector
+        # Thrust force vector
         # K_Prop scaled down by a factor of 10 to match exp. results
         tau_thrust = np.array([(1-t_prop) * X_prop, 0, 0, K_prop / 10, 0, 0], float)
 
-        tau_debug = tau_fins + tau_thrust
         # AUV dynamics
         tau_sum = tau_thrust + tau_fins + tau_liftdrag + tau_crossflow - np.matmul(C+D,nu_r)  - g
         nu_dot = Dnu_c + np.matmul(self.Minv, tau_sum)
@@ -382,7 +383,7 @@ def stepInput(self, t):
                          n          propeller revolution (rpm) ]
         """
         delta_r =  5 * self.D2R      # rudder angle (rad)
-        delta_s = 5 * self.D2R      # stern angle (rad)
+        delta_s = -5 * self.D2R      # stern angle (rad)
         n = 1525                     # propeller revolution (rpm)
 
         if t > 100: