BlueRobotics 2018b properties added

Author: edxmorgan

.DS_Store

@@ -1,4 +1,4 @@
-from casadi import SX, horzcat, inv, sin,cos, fabs, Function
+from casadi import SX, horzcat, inv, sin,cos, fabs, Function, diag, pinv
 from diffUV.base import Base
 from diffUV.utils.symbol import *
 from diffUV.utils.operators import cross_product
@@ -39,7 +39,7 @@ def get_inertia_matrix(self):
 
     def coriolis_centripetal_matrix(self):
         """Compute and return the Coriolis and centripetal matrix based on current vehicle state."""
-        M = self.inertia_matrix()
+        M = self.get_inertia_matrix()
         M11 = M[:3, :3]
         M12 = M[:3, 3:]
         M21 = M[3:, :3]
@@ -64,29 +64,36 @@ def gvect(self):
             sin(phi) - (y_g*W - y_b*B)*sin(thet)
         # For neutrally buoyant vehicles W = B
         return g
+    
+    def gvect_quat(self):
+        """Compute and return the hydrostatic restoring forces using quaternions."""
+        g_quat = SX(6, 1)
+        g_quat[0, 0] = (B-W)*(2*eps1*eps3 - 2*eps2*eta)
+        g_quat[1, 0] = (B-W)*(2*eps2*eps3 - 2*eps1*eta)
+        g_quat[2, 0] = (W-B)*(2*eps1**2 - 2*eps2**2 -1)
+        g_quat[3, 0] = z_g*W*(2*eps2*eps3 + 2*eps1*eta)
+        g_quat[4, 0] = z_g*W*(2*eps1*eps3 + 2*eps2*eta)
+        g_quat[5, 0] = 0
+        # For neutrally buoyant vehicles W = B
+        return g_quat
+
 
     def damping(self):
         """Compute and return the total damping forces, including both linear and nonlinear components."""
-        D = SX(6, 6)
-        D[0, 0] = X_u
-        D[1, 1] = Y_v
-        D[2, 2] = Z_w
-        D[3, 3] = K_p
-        D[4, 4] = M_q
-        D[5, 5] = N_r
-        linear_damping = D@x_nb
 
-        Dn_vars = [Dn1, Dn2, Dn3, Dn4, Dn5, Dn6]
-        Dn_list = [(fabs(x_nb).T @ Dn @ x_nb) for Dn in Dn_vars]
-        nonlinear_damping = vertcat(*Dn_list)
-
-        damping = linear_damping + nonlinear_damping
-        return damping
+        linear_damping = -diag(vertcat(X_u,Y_v,Z_w,K_p,M_q,N_r))
+        nonlinear_damping = -diag(vertcat(X_uu,Y_vv,Z_ww,K_pp,M_qq,N_rr))@fabs(x_nb)
+        D_v = linear_damping + nonlinear_damping
+        return D_v
 
     def forward_dynamics(self):
-        body_acc = inv(self.inertia_matrix())@(tau -self.coriolis_centripetal_matrix()@x_nb - self.gvect() -self.damping())
+        body_acc = inv(self.get_inertia_matrix())@(tau - self.coriolis_centripetal_matrix()@x_nb - self.gvect() -self.damping()@x_nb)
         return body_acc
 
     def inverse_dynamics(self):
-        resultant_torque = self.inertia_matrix()@dx_nb + self.coriolis_centripetal_matrix()@x_nb + self.gvect() + self.damping()
+        resultant_torque = self.get_inertia_matrix()@dx_nb + self.coriolis_centripetal_matrix()@x_nb + self.gvect() + self.damping()@x_nb
         return resultant_torque
+
+    def control_Allocation(self):
+        u = inv(K)@pinv(T)@tau
+        return u

diffUV/dynamics.py

@@ -0,0 +1,44 @@
+import numpy as np
+
+class Params:
+    thrust_config = np.array([[-0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0],
+        [0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 1.0, -1.0, -1.0, 1.0],
+        [0.0, 0.0, 0.0, 0.0, -0.218, -0.218, 0.218, 0.218],
+        [0.0, 0.0, 0.0, 0.0, -0.12, 0.12, -0.12, 0.12],
+        [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0, 0.0]])
+    
+    #thrust coefficient matrix
+    K = np.diag([40, 40, 40, 40, 40, 40, 40, 40])
+
+    # parameters in rigid body dynamics and restoring forces
+    m = 11.5 #(kg)
+    W = 112.8 #(N)
+    B = 114.8 #(N)
+    rb  = np.array([[0], [0], [0]]) #(m)
+    rg = np.array([[0], [0], [0.02]]) #(m)
+    I_x = 0.16 #(kg m2)
+    I_y = 0.16 #(kg m2)
+    I_z = 0.16 #(kg m2)
+
+    # added mass parameters
+    X_du = -5.5 #(kg)
+    Y_dv = -12.7 #(kg)
+    Z_dw = -14.57 #(kg)
+    K_dp = -0.12 #(kg m2/rad)
+    M_dq = -0.12 #(kg m2/rad)
+    N_dr = -0.12 #(kg m2/rad)
+
+    Xu = -4.03 #(Ns/m) 
+    Xuu = -18.18 #(Ns2/m2)
+    Yv = -6.22 #(Ns/m) 
+    Yvv = -21.66 #(Ns2/m2)
+    Zw = -5.18 #(Ns/m) 
+    Zww = -36.99 #(Ns2/m2)
+    Kp  = -0.07 #(Ns/rad) 
+    Kpp = -1.55 #(Ns2/rad2)
+    Mq = -0.07 #(Ns/rad)
+    Mqq = -1.55 #(Ns2/rad2)
+    Nr = -0.07 #(Ns/rad) 
+    Nrr = -1.55 #(Ns2/rad2)
+

diffUV/utils/parameters.py

@@ -126,12 +126,12 @@
 
 ###################################################################
 
-X_u = SX.sym('X_u') # Drag coefficient in surge
-Y_v = SX.sym('Y_v') # Drag coefficient in sway
-Z_w = SX.sym('Z_w') # Drag coefficient in heave
-K_p = SX.sym('K_p') # Drag coefficient in roll
-M_q = SX.sym('M_q') # Drag coefficient in pitch
-N_r = SX.sym('N_r') # Drag coefficient in yaw
+X_u = SX.sym('X_u') # linear Drag coefficient in surge
+Y_v = SX.sym('Y_v') # linear Drag coefficient in sway
+Z_w = SX.sym('Z_w') # linear Drag coefficient in heave
+K_p = SX.sym('K_p') # linear Drag coefficient in roll
+M_q = SX.sym('M_q') # linear Drag coefficient in pitch
+N_r = SX.sym('N_r') # linear Drag coefficient in yaw
 
 K_v = SX.sym('K_v') # coupled Drag coefficient in sway & roll
 N_v = SX.sym('N_v') # coupled Drag coefficient in sway & yaw
@@ -142,16 +142,20 @@
 Y_r = SX.sym('Y_r') # coupled Drag coefficient in sway & yaw
 K_r = SX.sym('K_r') # coupled Drag coefficient in roll & yaw
 
-Dn1 = SX.sym('Dn1',6,6) # nonlinear and coupled coefficient for surge
-Dn2 = SX.sym('Dn2',6,6) # nonlinear and coupled coefficient for sway
-Dn3 = SX.sym('Dn3',6,6) # nonlinear and coupled coefficient for heave
-Dn4 = SX.sym('Dn4',6,6) # nonlinear and coupled coefficient for roll
-Dn5 = SX.sym('Dn5',6,6) # nonlinear and coupled coefficient for pitch
-Dn6 = SX.sym('Dn6',6,6) # nonlinear and coupled coefficient for yaw
+
+X_uu = SX.sym('X_uu') # quadratic Drag coefficient in surge
+Y_vv = SX.sym('Y_vv') # quadratic Drag coefficient in sway
+Z_ww = SX.sym('Z_ww') # quadratic Drag coefficient in heave
+K_pp = SX.sym('K_pp') # quadratic Drag coefficient in roll
+M_qq = SX.sym('M_qq') # quadratic Drag coefficient in pitch
+N_rr = SX.sym('N_rr') # quadratic Drag coefficient in yaw
 
 ###################################################################
 tau = SX.sym('tau',6,1)
 
+K = SX.sym('k',6,6) #thrust coefficient matrix
+T = SX.sym('T',6,6) #thruster configuration
+
 ###################################################################
 # Starboard–port symmetrical underwater vehicles config
 star_board_config = DM([[1, 0, 1, 0, 1, 0],

diffUV/utils/symbol.py

@@ -0,0 +1,53 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 4,
+   "metadata": {},
+   "outputs": [
+    {
+     "ename": "ModuleNotFoundError",
+     "evalue": "No module named 'diffUV'",
+     "output_type": "error",
+     "traceback": [
+      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
+      "\u001b[0;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
+      "Cell \u001b[0;32mIn[4], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mdiffUV\u001b[39;00m\n",
+      "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'diffUV'"
+     ]
+    }
+   ],
+   "source": [
+    "import diffUV"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.11.7"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}