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