From 4bddb152cbaf607ca24f0303f93ed10b6ce197de Mon Sep 17 00:00:00 2001 From: Lucas-Carmo Date: Sat, 1 Jun 2024 18:05:30 -0600 Subject: [PATCH] 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 | 76 ++++++++++++++++++++++++++++++++++- moorpy/line.py | 57 ++++++++++++++++++++++++++ moorpy/point.py | 37 ++++------------- moorpy/system.py | 102 +++++++++++++++++++++++++++++++++++------------ 4 files changed, 217 insertions(+), 55 deletions(-) diff --git a/moorpy/body.py b/moorpy/body.py index 84bcccc..8c76314 100644 --- a/moorpy/body.py +++ b/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 diff --git a/moorpy/line.py b/moorpy/line.py index ffd917c..e77c6c2 100644 --- a/moorpy/line.py +++ b/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().''' diff --git a/moorpy/point.py b/moorpy/point.py index 358439a..031e11c 100644 --- a/moorpy/point.py +++ b/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.''' diff --git a/moorpy/system.py b/moorpy/system.py index e831c02..1443bb1 100644 --- a/moorpy/system.py +++ b/moorpy/system.py @@ -2877,9 +2877,15 @@ def getSystemStiffnessA(self, DOFtype="free", lines_only=False, rho=1025, g=9.81 return K - def getCoupledDynamicMatrices(self, omegas, S_zeta, depth, kbot, cbot, r_dynamic_init=None, lines_only=False): + def getCoupledDynamicMatrices(self, omegas, S_zeta, depth, kbot=0, cbot=0, r_dynamic_init=None, lines_only=False): '''Write something here later - ''' + ''' + # The methods used to get the coupled dynamic matrices do not work well if we treat each line section separately - I don't know why yet + # Hence, we group the lines into subsystems + + + + self.nDOF, self.nCpldDOF, DOFtypes = self.getDOFs() n = self.nDOF + self.nCpldDOF @@ -2888,13 +2894,13 @@ def getCoupledDynamicMatrices(self, omegas, S_zeta, depth, kbot, cbot, r_dynamic print("Getting mooring system stiffness matrix...") # get full system stiffness matrix - M_all, A_all, B_all, K_all = self.getSystemDynamicMatrices(omegas, S_zeta, r_dynamic_init, depth, kbot, cbot, DOFtype="both", lines_only=lines_only) + M_all, A_all, B_all, K_all = self.getSystemDynamicMatrices(omegas, S_zeta, depth, kbot, cbot, DOFtype="both", lines_only=lines_only, r_dynamic_init=r_dynamic_init) # invert matrix - M_inv_all = np.linalg.inv(M_all) - A_inv_all = np.linalg.inv(A_all) - B_inv_all = np.linalg.inv(B_all) - K_inv_all = np.linalg.inv(K_all) + M_inv_all = np.linalg.pinv(M_all) + A_inv_all = np.linalg.pinv(A_all) + B_inv_all = np.linalg.pinv(B_all) + K_inv_all = np.linalg.pinv(K_all) # remove free DOFs (this corresponds to saying that the sum of forces on these DOFs will remain zero) #indices = list(range(n)) # list of DOF indices that will remain active for this step @@ -2911,18 +2917,15 @@ def getCoupledDynamicMatrices(self, omegas, S_zeta, depth, kbot, cbot, r_dynamic K_inv_coupled = K_inv_all[mask,:][:,mask] # invert reduced matrix to get coupled stiffness matrix (with free DOFs assumed to equilibrate linearly) - M_coupled = np.linalg.inv(M_inv_coupled) - A_coupled = np.linalg.inv(A_inv_coupled) - B_coupled = np.linalg.inv(B_inv_coupled) - K_coupled = np.linalg.inv(K_inv_coupled) - - #if tensions: - # return K_coupled, J - #else: + M_coupled = np.linalg.pinv(M_inv_coupled) + A_coupled = np.linalg.pinv(A_inv_coupled) + B_coupled = np.linalg.pinv(B_inv_coupled) + K_coupled = np.linalg.pinv(K_inv_coupled) + return M_coupled, A_coupled, B_coupled, K_coupled - def getSystemDynamicMatrices(self, omegas, S_zeta, r_dynamic_init, depth, kbot, cbot, DOFtype="free", lines_only=False, rho=1025, g=9.81): + def getSystemDynamicMatrices(self, omegas, S_zeta, depth, kbot=0, cbot=0, DOFtype="free", lines_only=False, r_dynamic_init=None, rho=1025, g=9.81): '''Write something here later ''' @@ -2973,9 +2976,11 @@ def getSystemDynamicMatrices(self, omegas, S_zeta, r_dynamic_init, depth, kbot, #i = (body1.number-1)*6 # start counting index for body DOFs based on body number to keep indexing consistent # get body's self-stiffness matrix (now only cross-coupling terms will be handled on a line-by-line basis) - K6 = body1.getStiffnessA(lines_only=lines_only) - K[i:i+body1.nDOF, i:i+body1.nDOF] += K6 - + M6, A6, B6, K6 = body1.getDynamicMatrices(omegas, S_zeta, depth, kbot=kbot, cbot=cbot, r_dynamic_init=r_dynamic_init, lines_only=lines_only) + M[i:i+body1.nDOF, i:i+body1.nDOF] += M6 + A[i:i+body1.nDOF, i:i+body1.nDOF] += A6 + B[i:i+body1.nDOF, i:i+body1.nDOF] += B6 + K[i:i+body1.nDOF, i:i+body1.nDOF] += K6 # go through each attached point for pointID1,rPointRel1 in zip(body1.attachedP,body1.rPointRel): @@ -2984,17 +2989,20 @@ def getSystemDynamicMatrices(self, omegas, S_zeta, r_dynamic_init, depth, kbot, r1 = rotatePosition(rPointRel1, body1.r6[3:]) # relative position of Point about body ref point in unrotated reference frame H1 = getH(r1) # produce alternator matrix of current point's relative position to current body - for lineID in point1.attached: # go through each attached line to the Point, looking for when its other end is attached to something that moves endFound = 0 # simple flag to indicate when the other end's attachment has been found j = i + body1.nDOF # first index of the DOFs this line is attached to. Start it off at the next spot after body1's DOFs # get cross-coupling stiffness of line: force on end attached to body1 due to motion of other end - if point1.attachedEndB == 1: - KB = self.lineList[lineID-1].KBA - else: - KB = self.lineList[lineID-1].KBA.T + if r_dynamic_init == None: + r_init = np.ones((len(omegas),self.lineList[lineID-1].nNodes,3)) + M_all, A_all, B_all, K_all = self.lineList[lineID-1].getDynamicMatricesLumped(omegas,S_zeta,r_init,depth,kbot,cbot) + + MB = M_all[-3:,:3] if point1.attachedEndB == 1 else M_all[:3,-3:] + AB = A_all[-3:,:3] if point1.attachedEndB == 1 else A_all[:3,-3:] + BB = B_all[-3:,:3] if point1.attachedEndB == 1 else B_all[:3,-3:] + KB = K_all[-3:,:3] if point1.attachedEndB == 1 else K_all[:3,-3:] # look through Bodies further on in the list (coupling with earlier Bodies will already have been taken care of) for body2 in self.bodyList[self.bodyList.index(body1)+1: ]: @@ -3060,12 +3068,56 @@ def getSystemDynamicMatrices(self, omegas, S_zeta, r_dynamic_init, depth, kbot, # >>> TODO: handle case of free end point resting on seabed <<< # get point's self-stiffness matrix - M1, A1, B1, K1 = point.getDynamicMatrices(omegas, S_zeta, r_dynamic_init, depth, kbot, cbot) + M1, A1, B1, K1 = point.getDynamicMatrices(omegas, S_zeta, depth, kbot, cbot, r_dynamic_init=r_dynamic_init) M[i:i+n,i:i+n] += M1 A[i:i+n,i:i+n] += A1 B[i:i+n,i:i+n] += B1 K[i:i+n,i:i+n] += K1 + + for lineID in point.attached: + + j = i + n + + # go through movable points to see if one is attached + for point2 in self.pointList[self.pointList.index(point)+1: ]: + if point2.type in d: + if lineID in point2.attached: # if this point is at the other end of the line + + # get cross-coupling stiffness of line: force on end attached to point1 due to motion of other end + # The names are somewhat confusing, but AB: Added mass matrix (A) at point B due to motions of the other extremity + if r_dynamic_init == None: + r_init = np.ones((len(omegas), self.lineList[lineID-1].nNodes, 3)) + M_all, A_all, B_all, K_all = self.lineList[lineID-1].getDynamicMatricesLumped(omegas,S_zeta,r_init,depth,kbot,cbot) + MB = M_all[-3:,:3] if point.attachedEndB == 1 else M_all[:3,-3:] + AB = A_all[-3:,:3] if point.attachedEndB == 1 else A_all[:3,-3:] + BB = B_all[-3:,:3] if point.attachedEndB == 1 else B_all[:3,-3:] + KB = K_all[-3:,:3] if point.attachedEndB == 1 else K_all[:3,-3:] + + # Trim stiffness matrix to only use the enabled DOFs of each point + MB = MB[point.DOFs,:][:,point2.DOFs] + AB = AB[point.DOFs,:][:,point2.DOFs] + BB = BB[point.DOFs,:][:,point2.DOFs] + KB = KB[point.DOFs,:][:,point2.DOFs] + + # Include in total inertia matrix + M[i:i+n , j:j+point2.nDOF] += MB # force on P1 due to movement of P2 + M[j:j+point2.nDOF, i:i+n ] += MB.T # mirror (f on P2 due to x of P1) + + # Include in total added mass matrix + A[i:i+n , j:j+point2.nDOF] += AB # force on P1 due to movement of P2 + A[j:j+point2.nDOF, i:i+n ] += AB.T # mirror (f on P2 due to x of P1) + + # Include in total damping matrix + B[i:i+n , j:j+point2.nDOF] += BB # force on P1 due to movement of P2 + B[j:j+point2.nDOF, i:i+n ] += BB.T # mirror (f on P2 due to x of P1) + + # Include in total stiffness matrix + K[i:i+n , j:j+point2.nDOF] += KB # force on P1 due to movement of P2 + K[j:j+point2.nDOF, i:i+n ] += KB.T # mirror (f on P2 due to x of P1) + + j += point2.nDOF # if this point has DOFs we're considering, then count them + i += n return M, A, B, K