Functions to lump matrices at line ends and body + code reorganization

- Implemented getDynamicMatricesLumped method in the line class that lumps the added mass, inertia, damping, and stiffness matrices of a line at its extremities based on the matrices for each node (applied for a line modeled using a lumped mass model)
- Method getDynamicMatrices in the body class similar to getStiffnessA, but for the dynamic matrices
- Reorganized the code in point.getDynamicMatrices to use the new line.getDynamicMatricesLumped method
- Changed some np.liang.inv for np.linalg.pinv. We need that for cases where the matrices are singular. For instance, the added mass matrix of the line is singular if the axial added mass coefficient is zero.

WIP: There seems to be something wrong in line.getDynamicMatricesLumped if part of the line rests on the seabed, as the resulting stiffness matrix is not consistent with getStiffnessA. The agreement is much better when we totally remove those nodes, but they are still off. Need to figure out what is the correct way of dealing with that.

moorpy/body.py

@@ -370,8 +370,82 @@ def getStiffnessA(self, lines_only=False, all_DOFs=False):
             return K
         else:  # only return rows/columns of active DOFs
             return K[:,self.DOFs][self.DOFs,:]
-
+
+
+    def getDynamicMatrices(self, omegas, S_zeta, depth, kbot=0, cbot=0, r_dynamic_init=None, lines_only=False, all_DOFs=False):
+        '''Gets the dynamic matrices of the Body with other objects fixed
+        using a lumped mass approach.
+
+        Parameters
+        ----------
+        lines_only : boolean, optional
+            If true, the Body's contribution to its stiffness is ignored.
+        all_DOFs : boolean, optional
+            True: return all forces/moments; False: only those in DOFs list.
+
+        Returns
+        -------
+        K : matrix
+            6x6 analytic stiffness matrix.
+        '''
+
+        M, A, B, K = (np.zeros([6,6]) for _ in range(4))
+
+        # Contributions from attached points (and any of their attached lines)
+        for PointID,rPointRel in zip(self.attachedP,self.rPointRel):            
+            r = rotatePosition(rPointRel, self.r6[3:])          # relative position of Point about body ref point in unrotated reference frame  
+            f3 = self.sys.pointList[PointID-1].getForces()      # total force on point (for additional rotational stiffness term due to change in moment arm)
+
+            M3, A3, B3, K3 = self.sys.pointList[PointID-1].getDynamicMatrices(omegas, S_zeta, depth, kbot, cbot, r_dynamic_init=r_dynamic_init)  # local 3D dynamic matrices of the point
+
+            # following are from functions translateMatrix3to6            
+            H = getH(r)
+            K[:3,:3] += K3
+            K[:3,3:] += np.matmul(K3, H)                        # only add up one off-diagonal sub-matrix for now, then we'll mirror at the end
+            K[3:,3:] += -np.matmul(getH(f3), H) - np.matmul(H, np.matmul(K3,H))   # updated 2023-05-02
+
+            # Inertia, added mass, and damping matrices are similar. Except for the lower right submatrix, which does not have the first term because 
+            # the moment arm does not explictly change with the velocity or acceleration of the body.
+            M[:3,:3] += M3
+            M[:3,3:] += np.matmul(M3, H)
+            M[3:,3:] += - np.matmul(H, np.matmul(M3,H))
+
+            A[:3,:3] += A3
+            A[:3,3:] += np.matmul(A3, H)
+            A[3:,3:] += - np.matmul(H, np.matmul(A3,H))
+
+            B[:3,:3] += B3
+            B[:3,3:] += np.matmul(B3, H)
+            B[3:,3:] += - np.matmul(H, np.matmul(B3,H))            
+
+        K[3:,:3] = K[:3,3:].T                                   # copy over other off-diagonal sub-matrix
+        M[3:,:3] = M[:3,3:].T
+        A[3:,:3] = A[:3,3:].T
+        B[3:,:3] = B[:3,3:].T        
 
+        # body's own stiffness components
+        if lines_only == False:
+
+            # rotational stiffness effect of weight
+            rCG_rotated = rotatePosition(self.rCG, self.r6[3:]) # relative position of CG about body ref point in unrotated reference frame  
+            Kw = -np.matmul( getH([0,0, -self.m*self.sys.g]) , getH(rCG_rotated) )
+
+            # rotational stiffness effect of buoyancy at metacenter
+            rM_rotated = rotatePosition(self.rM, self.r6[3:])   # relative position of metacenter about body ref point in unrotated reference frame  
+            Kb = -np.matmul( getH([0,0, self.sys.rho*self.sys.g*self.v]) , getH(rM_rotated) )
+
+            # hydrostatic heave stiffness (if AWP is nonzero)
+            Kwp = self.sys.rho*self.sys.g*self.AWP
+
+            K[3:,3:] += Kw + Kb
+            K[2 ,2 ] += Kwp
+
+        # Return stiffness matrix
+        if all_DOFs:
+            return M, A, B, K
+        else:  # only return rows/columns of active DOFs
+            return M[:,self.DOFs][self.DOFs,:], A[:,self.DOFs][self.DOFs,:], B[:,self.DOFs][self.DOFs,:], K[:,self.DOFs][self.DOFs,:]
+
 
     def draw(self, ax):
         '''Draws the reference axis of the body

moorpy/line.py

@@ -1016,6 +1016,63 @@ def getDynamicMatrices(self, *args, **kwargs):
         '''Compute M,A,B,K matrices for Line. See get_dynamic_matrices().'''
         return get_dynamic_matrices(self, *args, **kwargs)
 
+    def getDynamicMatricesLumped(self, *args, **kwargs):
+        '''Lump M,A,B,K matrices for Line at its extremities, returning 6x6 matrices'''
+
+        def lump_matrix(matrix, nodes2remove=None):
+            # The matrices should be symmetrical, but they can be slightly off due to numerical errors.
+            # Because we are going to invert them twice, we force them to be symmetrical to avoid amplifying the errors. 
+            matrix = (matrix + matrix.T)/2
+            zeros = np.zeros((3,3))
+
+            # Remove rows and columns corresponding to the dofs of nodes2remove. Each node has 3 dofs
+            if nodes2remove.size==0:
+                nodes2remove = None
+
+            if nodes2remove is not None:
+                # Convert nodes2remove to dofs
+                dofs2remove = np.array([(node*3, node*3+1, node*3+2) for node in nodes2remove]).flatten()
+
+                # Create a mask for the dofs to keep
+                mask = np.ones(matrix.shape[0], dtype=bool)
+                mask[dofs2remove] = False
+                # Remove the rows and columns
+                matrix = matrix[mask][:, mask]
+
+            matrix_inv = np.linalg.pinv(matrix)
+
+            top_left     = matrix_inv[:3, :3]
+            top_right    = matrix_inv[:3, -3:]
+            bottom_left  = matrix_inv[-3:, :3]
+            bottom_right = matrix_inv[-3:, -3:]
+
+            # If we are not removing the extremities, we fill the whole 6x6 matrix_inv_coupled
+            if nodes2remove is None or (nodes2remove[0] !=0 and nodes2remove[-1] != self.nNodes-1):
+                matrix_inv_coupled = np.block([[top_left, top_right], [bottom_left, bottom_right]])
+
+            # if we are removing the first node, we fill the bottom right 3x3 matrix
+            if nodes2remove is not None and nodes2remove[0] == 0:                
+                    matrix_inv_coupled = np.block([[np.zeros((3,3)), np.zeros((3,3))], [np.zeros((3,3)), bottom_right]])
+
+            # if we are removing the last node, we fill the top left 3x3 matrix
+            if nodes2remove is not None and nodes2remove[-1] == self.nNodes-1:
+                matrix_inv_coupled = np.block([[top_left, np.zeros((3,3))], [np.zeros((3,3)), np.zeros((3,3))]])
+
+            return np.linalg.pinv(matrix_inv_coupled)
+
+        # Remove the nodes that are lying on the seabed
+        X_mean,Y_mean,Z_mean,T_mean = self.getLineCoords(0.0,n=self.nNodes) # coordinates of line nodes and tension values
+        idx2remove = np.where(Z_mean <= -self.sys.depth+1e-06)[0]
+
+        M, A, B, K, _, _ = self.getDynamicMatrices(*args, **kwargs)
+        Ml = lump_matrix(M, nodes2remove=idx2remove)
+        Al = lump_matrix(A, nodes2remove=idx2remove)
+        Bl = lump_matrix(B, nodes2remove=idx2remove)
+        Kl = lump_matrix(K, nodes2remove=idx2remove)
+        return Ml, Al, Bl, Kl
+
+
+
 
     def dynamicSolve(self, *args, **kwargs):
         '''Compute complex amplitudes of line nodes. See get_dynamic_tension().'''

moorpy/point.py

@@ -341,54 +341,33 @@ def getStiffnessA(self, lines_only=False, xyz=False):
         else:                       # otherwise only return rows/columns of active DOFs
             return K[:,self.DOFs][self.DOFs,:]
 
-    def getDynamicMatrices(self, omegas, S_zeta, r_dynamic_init, depth, kbot, cbot):
+    def getDynamicMatrices(self, omegas, S_zeta, depth, kbot=0, cbot=0, r_dynamic_init=None):
         '''Gets inertia, added mass, damping, and stiffness matrices of Point due only to mooring lines (with other objects fixed)
         using a lumped mass model.
 
         Returns
         -------
-        K : matrix
-            3x3 analytic stiffness matrix.
-
         '''
-
-        #print("Getting Point "+str(self.number)+" analytic stiffness matrix...")
-
-        def lump_matrix(matrix, endB):
-            # The matrices should be symmetrical, but they can be slightly off due to numerical errors.
-            # Because we are going to invert them twice, we force them to be symmetrical to avoid amplifying the errors. 
-            matrix = (matrix + matrix.T)/2
-            if endB == 1:            
-                matrix_woAnc = matrix[3:, 3:] # For some reason that I don't know yet, we need to remove the elements in the opposite side of the matrix. Otherwise it doesn't work
-                matrix_inv = np.linalg.pinv(matrix_woAnc)
-                matrix_inv_coupled = matrix_inv[-3:, -3:]
-            else:
-                matrix_woAnc = matrix[:-3, :-3]
-                matrix_inv = np.linalg.pinv(matrix_woAnc)
-                matrix_inv_coupled = matrix_inv[0:3, 0:3]
-            matrix_out = np.linalg.inv(matrix_inv_coupled)
-            return matrix_out 
-
         M = np.zeros([3,3])
         A = np.zeros([3,3])
         B = np.zeros([3,3])
         K = np.zeros([3,3])
-        
+
         # append the stiffness matrix of each line attached to the point
         for lineID,endB in zip(self.attached,self.attachedEndB):            
             line = self.sys.lineList[lineID-1]
             if r_dynamic_init == None:
                 r_init = np.ones((len(omegas),line.nNodes,3))
-            M_all, A_all, B_all, K_all, _, _ = line.getDynamicMatrices(omegas,S_zeta,r_init,depth,kbot,cbot)
+
+            M_all, A_all, B_all, K_all = line.getDynamicMatricesLumped(omegas,S_zeta,r_init,depth,kbot,cbot) 
 
-            M += lump_matrix(M_all, endB)
-            A += lump_matrix(A_all, endB)
-            B += lump_matrix(B_all, endB)
-            K += lump_matrix(K_all, endB)
+            M += M_all[-3:,-3:] if endB == 1 else M_all[:3, :3]
+            A += A_all[-3:,-3:] if endB == 1 else A_all[:3, :3]
+            B += B_all[-3:,-3:] if endB == 1 else B_all[:3, :3]
+            K += K_all[-3:,-3:] if endB == 1 else K_all[:3, :3]
 
         return M, A, B, K
 
-
     def getCost(self):
         '''Fill in and returns a cost dictionary for this Point object.
         So far it only applies for if the point is an anchor.'''