Source code for sharpy.structure.utils.lagrangeconstraints

"""
LagrangeConstraints library

Library used to create the matrices associated to boundary conditions through
the method of Lagrange Multipliers. The source code includes four different sections.

* Basic structures: basic functions and variables needed to organise the library with different Lagrange Constraints to enhance the interaction with this library.

* Auxiliar functions: basic queries that are performed repeatedly.

* Equations: functions that generate the equations associated to the constraint of basic degrees of freedom.

* Lagrange Constraints: different available Lagrange Constraints. They tipically use the basic functions in "Equations" to assembly the required set of equations.

Attributes:
    dict_of_lc (dict): Dictionary including the available Lagrange Contraint identifier
    (``_lc_id``) and the associated ``BaseLagrangeConstraint`` class

Notes:
    To use this library: import sharpy.structure.utils.lagrangeconstraints as lagrangeconstraints

Args:
    lc_list (list): list of all the defined contraints
    MBdict (dict): dictionary with the MultiBody and LagrangeMultipliers information
    MB_beam (list): list of :class:`~sharpy.structure.models.beam.Beam` of each of the bodies that form the system
    MB_tstep (list): list of :class:`~sharpy.utils.datastructures.StructTimeStepInfo` of each of the bodies that form the system
    num_LM_eq (int): number of new equations needed to define the boundary boundary conditions
    sys_size (int): total number of degrees of freedom of the multibody system
    dt (float): time step
    Lambda (np.ndarray): list of Lagrange multipliers values
    Lambda_dot (np.ndarray): list of the first derivative of the Lagrange multipliers values
    dynamic_or_static (str): string defining if the computation is dynamic or static
    LM_C (np.ndarray): Damping matrix associated to the Lagrange Multipliers equations
    LM_K (np.ndarray): Stiffness matrix associated to the Lagrange Multipliers equations
    LM_Q (np.ndarray): Vector of independent terms associated to the Lagrange Multipliers equations
"""
from abc import ABCMeta, abstractmethod
import sharpy.utils.cout_utils as cout
import os
import ctypes as ct
import numpy as np
import sharpy.utils.algebra as ag
from sharpy.utils.settings import set_value_or_default

###############################################################################
# Basic structures
###############################################################################

dict_of_lc = {}
lc = {}  # for internal working


# decorator
def lagrangeconstraint(arg):
    """
    Decorator used to create the dictionary (``dict_of_lc``) that links constraints id (``_lc_id``) to the associated ``BaseLagrangeConstraint`` class
    """
    global dict_of_lc
    try:
        arg._lc_id
    except AttributeError:
        raise AttributeError('Class defined as lagrange constraint has no _lc_id attribute')
    dict_of_lc[arg._lc_id] = arg
    return arg

def print_available_lc():
    """
    Prints the available Lagrange Constraints
    """
    cout.cout_wrap('The available lagrange constraints on this session are:', 2)
    for name, i_lc in dict_of_lc.items():
        cout.cout_wrap('%s ' % i_lc._lc_id, 2)

def lc_from_string(string):
    """
    Returns the ``BaseLagrangeConstraint`` class associated to a constraint id (``_lc_id``)
    """
    return dict_of_lc[string]

def lc_list_from_path(cwd):
    onlyfiles = [f for f in os.listdir(cwd) if os.path.isfile(os.path.join(cwd, f))]

    for i_file in range(len(onlyfiles)):
        if ".py" in onlyfiles[i_file]:
            if onlyfiles[i_file] == "__init__.py":
                onlyfiles[i_file] = ""
                continue
            onlyfiles[i_file] = onlyfiles[i_file].replace('.py', '')
        else:
            onlyfiles[i_file] = ""

    files = [file for file in onlyfiles if not file == ""]
    return files


def initialise_lc(lc_name, print_info=True):
    """
    Initialises the Lagrange Constraints
    """
    if print_info:
        cout.cout_wrap('Generating an instance of %s' % lc_name, 2)
    cls_type = lc_from_string(lc_name)
    lc = cls_type()
    return lc


[docs]class BaseLagrangeConstraint(metaclass=ABCMeta): __doc__ = """ BaseLagrangeConstraint Base class for LagrangeConstraints showing the methods required. They will be inherited by all the Lagrange Constraints Attributes: _n_eq (int): Number of equations required by a LagrangeConstraint _ieq (int): Number of the first equation associated to the Lagrange Constraint in the whole set of Lagrange equations """ _lc_id = 'BaseLagrangeConstraint' def __init__(self): """ Initialisation """ self._n_eq = None self._ieq = None
[docs] @abstractmethod def get_n_eq(self): """ Returns the number of equations required by the Lagrange Constraint """ return self._n_eq
[docs] @abstractmethod # def initialise(self, **kwargs): def initialise(self, MBdict_entry, ieq): """ Initialisation """ self._ieq = ieq return self._ieq + self._n_eq
[docs] @abstractmethod # def staticmat(self, **kwargs): def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): """ Generates the structural matrices (damping, stiffness) and the independent vector associated to the LagrangeConstraint in a static simulation """ return np.zeros((6, 6))
[docs] @abstractmethod # def dynamicmat(self, **kwargs): def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): """ Generates the structural matrices (damping, stiffness) and the independent vector associated to the LagrangeConstraint in a dynamic simulation """ return np.zeros((10, 10))
[docs] @abstractmethod # def staticpost(self, **kwargs): def staticpost(self, lc_list, MB_beam, MB_tstep): """ Postprocess operations needed by the LagrangeConstraint in a static simulation """ return
[docs] @abstractmethod # def dynamicpost(self, **kwargs): def dynamicpost(self, lc_list, MB_beam, MB_tstep): """ Postprocess operations needed by the LagrangeConstraint in a dynamic simulation """ return
################################################################################ # Auxiliar functions ################################################################################ def define_node_dof(MB_beam, node_body, num_node): """ define_node_dof Define the position of the first degree of freedom associated to a certain node Args: MB_beam(list): list of :class:`~sharpy.structure.models.beam.Beam` node_body(int): body to which the node belongs num_node(int): number os the node within the body Returns: node_dof(int): first degree of freedom associated to the node """ node_dof = 0 for ibody in range(node_body): node_dof += MB_beam[ibody].num_dof.value if MB_beam[ibody].FoR_movement == 'free': node_dof += 10 node_dof += 6*MB_beam[node_body].vdof[num_node] return node_dof def define_FoR_dof(MB_beam, FoR_body): """ define_FoR_dof Define the position of the first degree of freedom associated to a certain frame of reference Args: MB_beam(list): list of :class:`~sharpy.structure.models.beam.Beam` node_body(int): body to which the node belongs num_node(int): number os the node within the body Returns: node_dof(int): first degree of freedom associated to the node """ FoR_dof = 0 for ibody in range(FoR_body): FoR_dof += MB_beam[ibody].num_dof.value if MB_beam[ibody].FoR_movement == 'free': FoR_dof += 10 FoR_dof += MB_beam[FoR_body].num_dof.value return FoR_dof ################################################################################ # Equations ################################################################################ def equal_pos_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, inode_in_body, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q): """ This function generates the stiffness and damping matrices and the independent vector associated to a constraint that imposes equal positions between a node and a frame of reference See ``LagrangeConstraints`` for the description of variables Args: node_FoR_dof (int): position of the first degree of freedom of the FoR to which the "node" belongs node_dof (int): position of the first degree of freedom associated to the "node" FoR_body (int): body number of the "FoR" FoR_dof (int): position of the first degree of freedom associated to the "FoR" Note: this equation constitutes a holonomic constraint which is not currently supported. Check ``equal_lin_vel_node_FoR`` """ cout.cout_wrap("WARNING: this equation constitutes a holonomic constraint which is not currently supported. Check ``equal_lin_vel_node_FoR``", 3) num_LM_eq_specific = 3 Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Simplify notation node_cga = MB_tstep[node_body].cga() node_pos = MB_tstep[node_body].pos[inode_in_body, :] node_FoR_pos = MB_tstep[node_body].for_pos[0:3] FoR_pos = MB_tstep[FoR_body].for_pos[0:3] # if MB_beam[node_body].FoR_movement == 'free': B[:, node_FoR_dof:node_FoR_dof+3] = np.eye(3) B[:, node_dof:node_dof+3] = node_cga B[:, FoR_dof:FoR_dof+3] = -np.eye(3) LM_K[sys_size + ieq : sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor*B LM_K[:sys_size, sys_size + ieq : sys_size + ieq + num_LM_eq_specific] += scalingFactor*np.transpose(B) LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(B), Lambda[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(node_FoR_pos + np.dot(node_cga, node_pos) - FoR_pos) LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat, Lambda[ieq : ieq + num_LM_eq_specific]) if penaltyFactor: q = np.zeros((sys_size, )) q[node_FoR_dof:node_FoR_dof+3] = node_FoR_pos q[node_dof:node_dof+3] = node_pos q[FoR_dof:FoR_dof+3] = FoR_pos LM_Q[:sys_size] += penaltyFactor*np.dot(B.T, np.dot(B, q)) LM_K[node_FoR_dof:node_FoR_dof+3, node_FoR_dof:node_FoR_dof+3] += penaltyFactor*np.eye(3) LM_K[node_FoR_dof:node_FoR_dof+3, node_dof:node_dof+3] += penaltyFactor*node_cga LM_K[node_FoR_dof:node_FoR_dof+3, FoR_dof:FoR_dof+3] += -penaltyFactor*np.eye(3) LM_C[node_FoR_dof:node_FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*ag.der_Cquat_by_v(MB_tstep[node_body].quat, node_pos) LM_K[node_dof:node_dof+3, node_FoR_dof:node_FoR_dof+3] += penaltyFactor*node_cga.T LM_K[node_dof:node_dof+3, node_dof:node_dof+3] += penaltyFactor*np.eye(3) LM_K[node_dof:node_dof+3, FoR_dof:FoR_dof+3] += -penaltyFactor*node_cga.T LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*(ag.der_CquatT_by_v(MB_tstep[node_body].quat, node_FoR_pos - FoR_pos)) LM_K[FoR_dof:FoR_dof+3, node_FoR_dof:node_FoR_dof+3] += -penaltyFactor*np.eye(3) LM_K[FoR_dof:FoR_dof+3, node_dof:node_dof+3] += -penaltyFactor*node_cga.T LM_K[FoR_dof:FoR_dof+3, FoR_dof:FoR_dof+3] += penaltyFactor*np.eye(3) LM_C[FoR_dof:FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += -penaltyFactor*ag.der_Cquat_by_v(MB_tstep[node_body].quat, node_pos) ieq += 3 return ieq def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_posB = np.zeros((3))): """ This function generates the stiffness and damping matrices and the independent vector associated to a constraint that imposes equal linear velocities between a node and a frame of reference See ``LagrangeConstraints`` for the description of variables Args: node_number (int): number of the "node" within its own body node_body (int): body number of the "node" node_FoR_dof (int): position of the first degree of freedom of the FoR to which the "node" belongs node_dof (int): position of the first degree of freedom associated to the "node" FoR_body (int): body number of the "FoR" FoR_dof (int): position of the first degree of freedom associated to the "FoR" rel_posB (float np.array): relative position between the node and the FoR (in the node B FoR) """ num_LM_eq_specific = 3 Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Simplify notation node_cga = MB_tstep[node_body].cga() node_FoR_va = MB_tstep[node_body].for_vel[0:3] node_FoR_wa = MB_tstep[node_body].for_vel[3:6] ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] psi = MB_tstep[node_body].psi[ielem, inode_in_elem, :] node_cab = ag.crv2rotation(psi) node_Ra = MB_tstep[node_body].pos[node_number,:] + np.dot(node_cab, rel_posB) node_dot_Ra = MB_tstep[node_body].pos_dot[node_number,:] FoR_cga = MB_tstep[FoR_body].cga() FoR_va = MB_tstep[FoR_body].for_vel[0:3] Bnh[:, FoR_dof:FoR_dof+3] = FoR_cga Bnh[:, node_dof:node_dof+3] = -1.0*node_cga if MB_beam[node_body].FoR_movement == 'free': Bnh[:, node_FoR_dof:node_FoR_dof+3] = -1.0*node_cga Bnh[:, node_FoR_dof+3:node_FoR_dof+6] = np.dot(node_cga,ag.skew(node_Ra)) LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(np.dot(FoR_cga, FoR_va) + -1.0*np.dot(node_cga, node_dot_Ra + node_FoR_va + -1.0*np.dot(ag.skew(node_Ra), node_FoR_wa))) LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, Lambda_dot[ieq:ieq+num_LM_eq_specific]) if MB_beam[node_body].FoR_movement == 'free': LM_C[node_dof:node_dof+3,node_FoR_dof+6:node_FoR_dof+10] -= scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat, Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_C[node_FoR_dof:node_FoR_dof+3,node_FoR_dof+6:node_FoR_dof+10] -= scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat,Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_C[node_FoR_dof+3:node_FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(ag.skew(node_Ra).T, ag.der_CquatT_by_v(MB_tstep[node_body].quat, Lambda_dot[ieq:ieq+num_LM_eq_specific])) LM_K[node_FoR_dof+3:node_FoR_dof+6,node_dof:node_dof+3] += scalingFactor*ag.skew(np.dot(node_cga.T,Lambda_dot[ieq:ieq+num_LM_eq_specific])) if penaltyFactor: q = np.zeros((sys_size)) q[FoR_dof:FoR_dof+3] = FoR_va q[node_dof:node_dof+3] = node_dot_Ra if MB_beam[node_body].FoR_movement == 'free': q[node_FoR_dof:node_FoR_dof+3] = node_FoR_va q[node_FoR_dof+3:node_FoR_dof+6] = node_FoR_wa LM_Q[:sys_size] += penaltyFactor*np.dot(np.dot(Bnh.T, Bnh), q) LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh) # Derivatives wrt the FoR quaterion LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, np.dot(node_cga, node_dot_Ra + node_FoR_va + np.dot(ag.skew(node_Ra), node_FoR_wa))) LM_C[node_dof:node_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*np.dot(node_cga.T, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, FoR_va)) if MB_beam[node_body].FoR_movement == 'free': LM_C[node_FoR_dof:node_FoR_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*np.dot(node_cga.T, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, FoR_va)) mat = ag.multiply_matrices(ag.skew(node_Ra).T, node_cga.T) LM_C[node_FoR_dof+3:node_FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, FoR_va)) # Derivatives wrt the node quaternion if MB_beam[node_body].FoR_movement == 'free': vec = -node_dot_Ra - node_FoR_va + np.dot(ag.skew(node_Ra), node_FoR_wa) LM_C[FoR_dof:FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) derivative = -ag.der_CquatT_by_v(MB_tstep[node_body].quat, np.dot(FoR_cga, FoR_va)) LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*derivative LM_C[node_FoR_dof:node_FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*derivative LM_C[node_FoR_dof+3:node_FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] -= penaltyFactor*np.dot(ag.skew(node_Ra), derivative) # Derivatives wrt the node Ra LM_K[FoR_dof:FoR_dof+3, node_dof:node_dof+3] -= penaltyFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.skew(node_FoR_wa)) LM_K[node_dof:node_dof+3, node_dof:node_dof+3] += penaltyFactor*ag.skew(node_FoR_wa) if MB_beam[node_body].FoR_movement == 'free': LM_K[node_FoR_dof:node_FoR_dof+3, node_dof:node_dof+3] += penaltyFactor*ag.skew(node_FoR_wa) vec = ag.multiply_matrices(node_cga.T, FoR_cga, FoR_va) - node_dot_Ra - node_FoR_va LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof:node_dof+3] += penaltyFactor*ag.skew(vec) LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof:node_dof+3] -= penaltyFactor*ag.der_skewp_skewp_v(node_Ra, node_FoR_wa) ieq += 3 return ieq def rel_rot_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_vel=np.zeros((3))): """ This function generates the stiffness and damping matrices and the independent vector associated to a constraint that imposes equal rotation velocities between a node and a frame of reference See ``LagrangeConstraints`` for the description of variables Args: node_number (int): number of the "node" within its own body node_body (int): body number of the "node" node_FoR_dof (int): position of the first degree of freedom of the FoR to which the "node" belongs node_dof (int): position of the first degree of freedom associated to the "node" FoR_body (int): body number of the "FoR" FoR_dof (int): position of the first degree of freedom associated to the "FoR" rel_vel (np.array): relative velocity FoR-node """ num_LM_eq_specific = 3 Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Simplify notation ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] node_cga = MB_tstep[node_body].cga() node_FoR_wa = MB_tstep[node_body].for_vel[3:6] psi = MB_tstep[node_body].psi[ielem,inode_in_elem,:] cab = ag.crv2rotation(psi) tan = ag.crv2tan(psi) FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] Bnh[:, node_dof+3:node_dof+6] += tan.copy() Bnh[:, FoR_dof+3:FoR_dof+6] -= ag.multiply_matrices(cab.T, node_cga.T, FoR_cga) if MB_beam[node_body].FoR_movement == 'free': Bnh[:, node_FoR_dof+3:node_FoR_dof+6] += cab.T LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(np.dot(tan, MB_tstep[node_body].psi_dot[ielem, inode_in_elem, :]) + np.dot(cab.T, node_FoR_wa) - ag.multiply_matrices(cab.T, node_cga.T, FoR_cga, FoR_wa) + rel_vel) LM_K[node_dof+3:node_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.der_TanT_by_xv(psi, Lambda_dot[ieq:ieq+num_LM_eq_specific]) if MB_beam[node_body].FoR_movement == 'free': LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.der_Ccrv_by_v(psi, Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] -= scalingFactor*ag.der_Ccrv_by_v(psi, ag.multiply_matrices(node_cga, FoR_cga.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])) LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] -= scalingFactor*np.dot(cab, ag.der_Cquat_by_v(MB_tstep[node_body].quat, np.dot(FoR_cga.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]))) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] -= scalingFactor*ag.multiply_matrices(cab, node_cga, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, Lambda_dot[ieq:ieq+num_LM_eq_specific])) ieq += 3 return ieq def def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, indep): """ This function generates the stiffness and damping matrices and the independent vector associated to a joint that forces the rotation axis of a FoR to be parallel to a certain direction. This direction is defined in the B FoR of a node and, thus, might change along the simulation. See ``LagrangeConstraints`` for the description of variables Args: rot_axisB (np.ndarray): Rotation axis with respect to the node B FoR indep (np.ndarray): Number of the equations that are used as independent node_number (int): number of the "node" within its own body node_body (int): body number of the "node" node_FoR_dof (int): position of the first degree of freedom of the FoR to which the "node" belongs node_dof (int): position of the first degree of freedom associated to the "node" FoR_body (int): body number of the "FoR" FoR_dof (int): position of the first degree of freedom associated to the "FoR" Notes: this function is missing the contribution of the rotation velocity of the reference node. See ``def_rot_axis_FoR_wrt_node_general`` """ cout.cout_wrap("WARNING: this function is missing the contribution of the rotation velocity of the reference node. See ``def_rot_axis_FoR_wrt_node_general``", 3) ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] # Simplify notation cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]) node_cga = MB_tstep[node_body].cga() FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] if not indep: aux_Bnh = ag.multiply_matrices(ag.skew(rot_axisB), cab.T, node_cga.T, FoR_cga) # indep = None n0 = np.linalg.norm(aux_Bnh[0,:]) n1 = np.linalg.norm(aux_Bnh[1,:]) n2 = np.linalg.norm(aux_Bnh[2,:]) if ((n0 < n1) and (n0 < n2)): indep[:] = [1, 2] elif ((n1 < n0) and (n1 < n2)): indep[:] = [0, 2] elif ((n2 < n0) and (n2 < n1)): indep[:] = [0, 1] new_Lambda_dot = np.zeros(3) new_Lambda_dot[indep[0]] = Lambda_dot[ieq] new_Lambda_dot[indep[1]] = Lambda_dot[ieq+1] num_LM_eq_specific = 2 Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') Bnh[:, FoR_dof+3:FoR_dof+6] = ag.multiply_matrices(ag.skew(rot_axisB), cab.T, node_cga.T, FoR_cga)[indep,:] # Constrain angular velocities LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(ag.skew(rot_axisB), cab.T, node_cga.T, FoR_cga, FoR_wa)[indep] LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) if MB_beam[node_body].FoR_movement == 'free': LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, ag.multiply_matrices(cab, ag.skew(rot_axisB).T, new_Lambda_dot))) LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, ag.multiply_matrices(node_cga, cab, ag.skew(rot_axisB).T, new_Lambda_dot)) LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, node_cga, cab, ag.skew(rot_axisB).T, new_Lambda_dot) if penaltyFactor: q = np.zeros((sys_size,)) q[FoR_dof+3:FoR_dof+6] = MB_tstep[FoR_body].for_vel[3:6] LM_Q[:sys_size] += penaltyFactor*np.dot(Bnh.T, np.dot(Bnh, q)) LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh) sq_rot_axisB = np.dot(ag.skew(rot_axisB).T, ag.skew(rot_axisB)) # Derivatives with the quaternion of the FoR vec = ag.multiply_matrices(node_cga, cab, sq_rot_axisB, cab.T, node_cga.T, FoR_cga, FoR_wa) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) mat = ag.multiply_matrices(FoR_cga.T, node_cga, cab, sq_rot_axisB, cab.T, node_cga.T) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, FoR_wa)) if MB_beam[node_body].FoR_movement == 'free': # Derivatives with the quaternion of the FoR of the node vec = ag.multiply_matrices(cab, sq_rot_axisB, cab.T, node_cga.T, FoR_cga, FoR_wa) LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) mat = ag.multiply_matrices(FoR_cga.T, node_cga, cab, sq_rot_axisB, cab.T) vec = np.dot(FoR_cga, FoR_wa) LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec)) # Derivatives with the CRV mat = np.dot(FoR_cga.T, node_cga) vec = ag.multiply_matrices(sq_rot_axisB, cab.T, node_cga.T, FoR_cga, FoR_wa) LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec)) mat = ag.multiply_matrices(FoR_cga.T, node_cga, cab, sq_rot_axisB) vec = ag.multiply_matrices(node_cga.T, FoR_cga, FoR_wa) LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_CcrvT_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec)) ieq += 2 return ieq def def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, zero_comp): """ This function generates the stiffness and damping matrices and the independent vector associated to a joint that forces the rotation axis of a FoR to be parallel to a certain direction. This direction is defined in the B FoR of a node and parallel to x, y or z See ``LagrangeConstraints`` for the description of variables Args: rot_axisB (np.ndarray): Rotation axis with respect to the node B FoR indep (np.ndarray): Number of the equations that are used as independent node_number (int): number of the "node" within its own body node_body (int): body number of the "node" node_FoR_dof (int): position of the first degree of freedom of the FoR to which the "node" belongs node_dof (int): position of the first degree of freedom associated to the "node" FoR_body (int): body number of the "FoR" FoR_dof (int): position of the first degree of freedom associated to the "FoR" """ ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] num_LM_eq_specific = 2 Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Simplify notation cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]) node_cga = MB_tstep[node_body].cga() FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] psi = MB_tstep[node_body].psi[ielem,inode_in_elem,:] psi_dot = MB_tstep[node_body].psi_dot[ielem,inode_in_elem,:] # Components to be zero Z = np.zeros((2,3)) Z[:, zero_comp] = np.eye(2) Bnh[:, FoR_dof+3:FoR_dof+6] += ag.multiply_matrices(Z, cab.T, node_cga.T, FoR_cga) Bnh[:, node_dof+3:node_dof+6] -= ag.multiply_matrices(Z, ag.crv2tan(psi)) Bnh[:, node_FoR_dof+3:node_FoR_dof+6] -= ag.multiply_matrices(Z, cab.T) # Constrain angular velocities LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(Z, cab.T, node_cga.T, FoR_cga, FoR_wa) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] -= scalingFactor*ag.multiply_matrices(Z, ag.crv2tan(psi), psi_dot) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] -= scalingFactor*ag.multiply_matrices(Z, cab.T, MB_tstep[node_body].for_vel[3:6]) LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) vec = ag.multiply_matrices(node_cga, cab, Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) if MB_beam[node_body].FoR_movement == 'free': vec = ag.multiply_matrices(cab, Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*ag.multiply_matrices(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], np.dot(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]))) LM_K[node_dof+3:node_dof+6, node_dof+3:node_dof+6] -= scalingFactor*ag.der_TanT_by_xv(psi, ag.multiply_matrices(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])) LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof+3:node_dof+6] -= scalingFactor*ag.der_Ccrv_by_v(psi, ag.multiply_matrices(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])) if penaltyFactor: q = np.zeros((sys_size,)) q[FoR_dof+3:FoR_dof+6] = FoR_wa LM_Q[:sys_size] += penaltyFactor*np.dot(Bnh.T, np.dot(Bnh, q)) LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh) ZTZ = np.dot(Z.T, Z) # Derivatives with the quaternion of the FoR vec = ag.multiply_matrices(node_cga, cab, ZTZ, cab.T, node_cga.T, FoR_cga, FoR_wa) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) mat = ag.multiply_matrices(FoR_cga.T, node_cga, cab, ZTZ, cab.T, node_cga.T) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, FoR_wa)) if MB_beam[node_body].FoR_movement == 'free': # Derivatives with the quaternion of the FoR of the node vec = ag.multiply_matrices(cab, ZTZ, cab.T, node_cga.T, FoR_cga, FoR_wa) LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) mat = ag.multiply_matrices(FoR_cga.T, node_cga, cab, ZTZ, cab.T) vec = np.dot(FoR_cga, FoR_wa) LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec)) # Derivatives with the CRV mat = np.dot(FoR_cga.T, node_cga) vec = ag.multiply_matrices(ZTZ, cab.T, node_cga.T, FoR_cga, FoR_wa) LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec)) mat = ag.multiply_matrices(FoR_cga.T, node_cga, cab, ZTZ) vec = ag.multiply_matrices(node_cga.T, FoR_cga, FoR_wa) LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_CcrvT_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec)) ieq += 2 return ieq def def_rot_vel_mod_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, nonzero_comp, rot_vel, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q): """ This function generates the stiffness and damping matrices and the independent vector associated to a joint that forces the rotation velocity of a FoR with respect to a node See ``LagrangeConstraints`` for the description of variables Args: nonzero_comp (int): Component of the rotation axis with respect to the node B FoR which is non-zero rot_vel (float): Rotation velocity node_number (int): number of the "node" within its own body node_body (int): body number of the "node" node_FoR_dof (int): position of the first degree of freedom of the FoR to which the "node" belongs node_dof (int): position of the first degree of freedom associated to the "node" FoR_body (int): body number of the "FoR" FoR_dof (int): position of the first degree of freedom associated to the "FoR" """ ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] num_LM_eq_specific = 1 Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Simplify notation cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]) node_cga = MB_tstep[node_body].cga() FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] # Components to be zero Znon = np.zeros((1,3)) Znon[:, nonzero_comp] = 1 Bnh[:, FoR_dof+3:FoR_dof+6] += ag.multiply_matrices(Znon, cab.T, node_cga.T, FoR_cga) # Constrain angular velocities LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(Znon, cab.T, node_cga.T, FoR_cga, FoR_wa) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] -= scalingFactor*rot_vel LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) vec = ag.multiply_matrices(node_cga, cab, Znon.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) if MB_beam[node_body].FoR_movement == 'free': vec = ag.multiply_matrices(cab, Znon.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*ag.multiply_matrices(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], np.dot(Znon.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]))) ieq += 1 return ieq def def_rot_vect_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_vect, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q): """ This function fixes the rotation velocity VECTOR of a FOR equal to a velocity vector defined in the B FoR of a node This function is a new implementation that combines and simplifies the use of 'def_rot_vel_mod_FoR_wrt_node' and 'def_rot_axis_FoR_wrt_node' together """ num_LM_eq_specific = 3 Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Simplify notation ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] node_cga = MB_tstep[node_body].cga() cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]) FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] Bnh[:, FoR_dof+3:FoR_dof+6] = ag.multiply_matrices(cab.T, node_cga.T, FoR_cga) # Constrain angular velocities LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(np.dot(Bnh[:, FoR_dof+3:FoR_dof+6], FoR_wa) - rot_vect) LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) if MB_beam[node_body].FoR_movement == 'free': LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, np.dot(cab, Lambda_dot[ieq:ieq+num_LM_eq_specific]))) LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, ag.multiply_matrices(node_cga, cab, Lambda_dot[ieq:ieq+num_LM_eq_specific])) LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], Lambda_dot[ieq:ieq+num_LM_eq_specific])) if penaltyFactor: LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += penaltyFactor*np.eye(3) q = np.zeros((sys_size)) q[FoR_dof+3:FoR_dof+6] = FoR_wa LM_Q[:sys_size] += penaltyFactor*np.dot(np.dot(Bnh.T, Bnh), q) ieq += 3 return ieq ################################################################################ # Lagrange constraints ################################################################################
[docs]@lagrangeconstraint class hinge_node_FoR(BaseLagrangeConstraint): __doc__ = """ hinge_node_FoR This constraint forces a hinge behaviour between a node and a FoR See ``LagrangeConstraints`` for the description of variables Attributes: node_number (int): number of the "node" within its own body node_body (int): body number of the "node" FoR_body (int): body number of the "FoR" rot_axisB (np.ndarray): Rotation axis with respect to the node B FoR """ _lc_id = 'hinge_node_FoR' def __init__(self): self.required_parameters = ['node_in_body', 'body', 'body_FoR', 'rot_axisB'] self._n_eq = 5
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.node_number = MBdict_entry['node_in_body'] self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] self.rot_axisB = MBdict_entry['rot_axisB'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) if (self.rot_axisB[[1, 2]] == 0).all(): self.rot_dir = 'x' self.zero_comp = np.array([1, 2], dtype=int) elif (self.rot_axisB[[0, 2]] == 0).all(): self.rot_dir = 'y' self.zero_comp = np.array([0, 2], dtype=int) elif (self.rot_axisB[[0, 1]] == 0).all(): self.rot_dir = 'z' self.zero_comp = np.array([0, 1], dtype=int) else: raise NotImplementedError("Hinges should be parallel to the xB, yB or zB of the reference node") #self.rot_dir = 'general' #self.indep = [] return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) node_FoR_dof = define_FoR_dof(MB_beam, self.node_body) FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq # Define the equations # ieq = equal_pos_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) if self.rot_dir == 'general': ieq = def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.indep) else: ieq = def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.zero_comp) return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(MB_tstep[self.node_body].cga(), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3] return
[docs]@lagrangeconstraint class hinge_node_FoR_constant_vel(BaseLagrangeConstraint): __doc__ = """ hinge_node_FoR_constant_vel This constraint forces a hinge behaviour between a node and a FoR and a constant rotation velocity at the join See ``LagrangeConstraints`` for the description of variables Attributes: node_number (int): number of the "node" within its own body node_body (int): body number of the "node" FoR_body (int): body number of the "FoR" rot_vect (np.ndarray): Rotation velocity vector in the node B FoR rel_posB (np.ndarray): Relative position between the node and the frame of reference in the node B FoR """ _lc_id = 'hinge_node_FoR_constant_vel' def __init__(self): self.required_parameters = ['node_in_body', 'body', 'body_FoR', 'rot_vect', 'rel_posB'] self._n_eq = 6
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.node_number = MBdict_entry['node_in_body'] self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] self.rel_posB = MBdict_entry['rel_posB'] self._ieq = ieq self.indep = [] self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) self.rot_axisB = ag.unit_vector(MBdict_entry['rot_vect']) if (self.rot_axisB[[1, 2]] == 0).all(): self.rot_dir = 'x' self.zero_comp = np.array([1, 2], dtype=int) self.nonzero_comp = 0 elif (self.rot_axisB[[0, 2]] == 0).all(): self.rot_dir = 'y' self.zero_comp = np.array([0, 2], dtype=int) self.nonzero_comp = 1 elif (self.rot_axisB[[0, 1]] == 0).all(): self.rot_dir = 'z' self.zero_comp = np.array([0, 1], dtype=int) self.nonzero_comp = 2 else: raise NotImplementedError("Hinges should be parallel to the xB, yB or zB of the reference node") self.set_rot_vel(MBdict_entry['rot_vect'][self.nonzero_comp]) return self._ieq + self._n_eq
def set_rot_vel(self, rot_vel): self.rot_vel = rot_vel
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) node_FoR_dof = define_FoR_dof(MB_beam, self.node_body) FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq # Define the equations # ieq = equal_pos_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) # ieq = def_rot_vect_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_vect, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) ieq = def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.zero_comp) ieq = def_rot_vel_mod_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.nonzero_comp, self.rot_vel, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): ielem, inode_in_elem = MB_beam[self.node_body].node_master_elem[self.node_number] node_cga = MB_tstep[self.node_body].cga() cab = ag.crv2rotation(MB_tstep[self.node_body].psi[ielem, inode_in_elem, :]) MB_tstep[self.FoR_body].for_pos[0:3] = (np.dot(node_cga, MB_tstep[self.node_body].pos[self.node_number,:] + np.dot(cab, self.rel_posB)) + MB_tstep[self.node_body].for_pos[0:3]) return
@lagrangeconstraint class hinge_node_FoR_pitch(BaseLagrangeConstraint): __doc__ = """ hinge_node_FoR_pitch This constraint forces a hinge behaviour between a node and a FoR and a rotation velocity at the joint See ``LagrangeConstraints`` for the description of variables Attributes: node_number (int): number of the "node" within its own body node_body (int): body number of the "node" FoR_body (int): body number of the "FoR" rot_vect (np.ndarray): Rotation velocity vector in the node B FoR rel_posB (np.ndarray): Relative position between the node and the frame of reference in the node B FoR """ _lc_id = 'hinge_node_FoR_pitch' def __init__(self): self.required_parameters = ['node_in_body', 'body', 'body_FoR', 'rotor_vel', 'rel_posB'] self._n_eq = 6 def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): self.node_number = MBdict_entry['node_in_body'] self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] self.rel_posB = MBdict_entry['rel_posB'] self._ieq = ieq self.indep = [] self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) self.set_rotor_vel(MBdict_entry['rotor_vel']) pitch_vel = set_value_or_default(MBdict_entry, "pitch_vel", 0.) self.set_pitch_vel(pitch_vel) return self._ieq + self._n_eq def set_rotor_vel(self, rotor_vel): self.rotor_vel = rotor_vel def set_pitch_vel(self, pitch_vel): self.pitch_vel = pitch_vel def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) node_FoR_dof = define_FoR_dof(MB_beam, self.node_body) FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq # Compute relative velocity ielem, inode_in_elem = MB_beam[self.node_body].node_master_elem[self.node_number] node_cga = MB_tstep[self.node_body].cga() cab = ag.crv2rotation(MB_tstep[self.node_body].psi[ielem, inode_in_elem, :]) FoR_cga = MB_tstep[self.FoR_body].cga() # rel_vel in B FoR rel_vel = np.array([0., 0., self.rotor_vel]) rel_vel += ag.multiply_matrices(cab.T, node_cga.T, FoR_cga, np.array([self.pitch_vel, 0., 0.])) # Define the equations ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) ieq = rel_rot_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_vel=rel_vel) return def staticpost(self, lc_list, MB_beam, MB_tstep): return def dynamicpost(self, lc_list, MB_beam, MB_tstep): ielem, inode_in_elem = MB_beam[self.node_body].node_master_elem[self.node_number] node_cga = MB_tstep[self.node_body].cga() cab = ag.crv2rotation(MB_tstep[self.node_body].psi[ielem, inode_in_elem, :]) MB_tstep[self.FoR_body].for_pos[0:3] = (np.dot(node_cga, MB_tstep[self.node_body].pos[self.node_number,:] + np.dot(cab, self.rel_posB)) + MB_tstep[self.node_body].for_pos[0:3]) return
[docs]@lagrangeconstraint class spherical_node_FoR(BaseLagrangeConstraint): __doc__ = """ spherical_node_FoR This constraint forces a spherical join between a node and a FoR See ``LagrangeConstraints`` for the description of variables Attributes: node_number (int): number of the "node" within its own body node_body (int): body number of the "node" FoR_body (int): body number of the "FoR" """ _lc_id = 'spherical_node_FoR' def __init__(self): self.required_parameters = ['node_in_body', 'body', 'body_FoR'] self._n_eq = 3
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.node_number = MBdict_entry['node_in_body'] self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) node_FoR_dof = define_FoR_dof(MB_beam, self.node_body) FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq # Define the equations ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(MB_tstep[self.node_body].cga(), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3] return
[docs]@lagrangeconstraint class free(BaseLagrangeConstraint): _lc_id = 'free' __doc__ = _lc_id def __init__(self): self.required_parameters = [] self._n_eq = 0
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self._ieq = ieq return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
[docs]@lagrangeconstraint class spherical_FoR(BaseLagrangeConstraint): __doc__ = """ spherical_FoR This constraint forces a spherical join at a FoR See ``LagrangeConstraints`` for the description of variables Attributes: body_FoR (int): body number of the "FoR" """ _lc_id = 'spherical_FoR' def __init__(self): self.required_parameters = ['body_FoR'] self._n_eq = 3
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.body_FoR = MBdict_entry['body_FoR'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.body_FoR) ieq = self._ieq Bnh[:3, FoR_dof:FoR_dof+3] = 1.0*np.eye(3) LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh) LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[0:3].astype(dtype=ct.c_double, copy=True, order='F') ieq += 3 return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
[docs]@lagrangeconstraint class hinge_FoR(BaseLagrangeConstraint): __doc__ = """ hinge_FoR This constraint forces a hinge at a FoR See ``LagrangeConstraints`` for the description of variables Attributes: body_FoR (int): body number of the "FoR" rot_axis_AFoR (np.ndarray): Rotation axis with respect to the node A FoR """ _lc_id = 'hinge_FoR' def __init__(self): self.required_parameters = ['body_FoR', 'rot_axis_AFoR'] self._n_eq = 5
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.body_FoR = MBdict_entry['body_FoR'] self.rot_axis = MBdict_entry['rot_axis_AFoR'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) if (self.rot_axis[[1, 2]] == 0).all(): self.rot_dir = 'x' self.zero_comp = np.array([1, 2], dtype=int) elif (self.rot_axis[[0, 2]] == 0).all(): self.rot_dir = 'y' self.zero_comp = np.array([0, 2], dtype=int) elif (self.rot_axis[[0, 1]] == 0).all(): self.rot_dir = 'z' self.zero_comp = np.array([0, 1], dtype=int) else: self.rot_dir = 'general' return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.body_FoR) ieq = self._ieq Bnh[:3, FoR_dof:FoR_dof+3] = 1.0*np.eye(3) if self.rot_dir == 'general': # Only two of these equations are linearly independent skew_rot_axis = ag.skew(self.rot_axis) n0 = np.linalg.norm(skew_rot_axis[0,:]) n1 = np.linalg.norm(skew_rot_axis[1,:]) n2 = np.linalg.norm(skew_rot_axis[2,:]) if ((n0 < n1) and (n0 < n2)): row0 = 1 row1 = 2 elif ((n1 < n0) and (n1 < n2)): row0 = 0 row1 = 2 elif ((n2 < n0) and (n2 < n1)): row0 = 0 row1 = 1 Bnh[3:5, FoR_dof+3:FoR_dof+6] = skew_rot_axis[[row0,row1],:] else: Bnh[3:5, FoR_dof+3+self.zero_comp] = np.eye(2) LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh) LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[0:3].astype(dtype=ct.c_double, copy=True, order='F') if self.rot_dir == 'general': LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*np.dot(skew_rot_axis[[row0,row1],:], MB_tstep[self.body_FoR].for_vel[3:6]) else: LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[3 + self.zero_comp] if self.penaltyFactor: LM_Q[FoR_dof:FoR_dof+3] += self.penaltyFactor*MB_tstep[self.body_FoR].for_vel[0:3] LM_C[FoR_dof:FoR_dof+3, FoR_dof:FoR_dof+3] += self.penaltyFactor*np.eye(3) if self.rot_dir == 'general': sq_rot_axis = np.dot(ag.skew(self.rot_axis).T, ag.skew(self.rot_axis)) LM_Q[FoR_dof+3:FoR_dof+6] += self.penaltyFactor*np.dot(sq_rot_axis, MB_tstep[self.body_FoR].for_vel[3:6]) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += self.penaltyFactor*sq_rot_axis else: LM_Q[FoR_dof+3:FoR_dof+6] += self.penaltyFactor*MB_tstep[self.body_FoR].for_vel[3:6] LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += self.penaltyFactor*np.eye(3) ieq += 5 return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
[docs]@lagrangeconstraint class hinge_FoR_wrtG(BaseLagrangeConstraint): __doc__ = """ hinge_FoR_wrtG This constraint forces a hinge at a FoR See ``LagrangeConstraints`` for the description of variables Attributes: body_FoR (int): body number of the "FoR" rot_axis_AFoR (np.ndarray): Rotation axis with respect to the node G FoR """ _lc_id = 'hinge_FoR_wrtG' def __init__(self): self.required_parameters = ['body_FoR', 'rot_axis_AFoR'] self._n_eq = 5
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.body_FoR = MBdict_entry['body_FoR'] self.rot_axis = MBdict_entry['rot_axis_AFoR'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.body_FoR) ieq = self._ieq Bnh[:3, FoR_dof:FoR_dof+3] = MB_tstep[self.body_FoR].cga() # Only two of these equations are linearly independent skew_rot_axis = ag.skew(self.rot_axis) n0 = np.linalg.norm(skew_rot_axis[0,:]) n1 = np.linalg.norm(skew_rot_axis[1,:]) n2 = np.linalg.norm(skew_rot_axis[2,:]) if ((n0 < n1) and (n0 < n2)): row0 = 1 row1 = 2 elif ((n1 < n0) and (n1 < n2)): row0 = 0 row1 = 2 elif ((n2 < n0) and (n2 < n1)): row0 = 0 row1 = 1 Bnh[3:5, FoR_dof+3:FoR_dof+6] = skew_rot_axis[[row0,row1],:] LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh) LM_C[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_FoR].quat,Lambda_dot[ieq:ieq+3]) LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*np.dot(MB_tstep[self.body_FoR].cga(),MB_tstep[self.body_FoR].for_vel[0:3]) LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*np.dot(skew_rot_axis[[row0,row1],:], MB_tstep[self.body_FoR].for_vel[3:6]) ieq += 5 return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
[docs]@lagrangeconstraint class fully_constrained_node_FoR(BaseLagrangeConstraint): __doc__ = """ fully_constrained_node_FoR This constraint forces linear and angular displacements between a node and a FoR to be the same See ``LagrangeConstraints`` for the description of variables Attributes: node_number (int): number of the "node" within its own body node_body (int): body number of the "node" FoR_body (int): body number of the "FoR" """ _lc_id = 'fully_constrained_node_FoR' def __init__(self): self.required_parameters = ['node_in_body', 'body', 'body_FoR', 'rel_posB'] self._n_eq = 6
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): # cout.cout_wrap("WARNING: do not use fully_constrained_node_FoR. It is outdated. Definetly not working if 'body' has velocity", 3) self.node_number = MBdict_entry['node_in_body'] self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] self.rel_posB = MBdict_entry['rel_posB'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) node_FoR_dof = define_FoR_dof(MB_beam, self.node_body) FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq # Define the equations ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) ieq = rel_rot_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_vel=np.zeros((3))) return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): ielem, inode_in_elem = MB_beam[self.node_body].node_master_elem[self.node_number] node_cga = MB_tstep[self.node_body].cga() cab = ag.crv2rotation(MB_tstep[self.node_body].psi[ielem, inode_in_elem, :]) MB_tstep[self.FoR_body].for_pos[0:3] = (np.dot(node_cga, MB_tstep[self.node_body].pos[self.node_number,:] + np.dot(cab, self.rel_posB)) + MB_tstep[self.node_body].for_pos[0:3]) return
[docs]@lagrangeconstraint class constant_rot_vel_FoR(BaseLagrangeConstraint): __doc__ = """ constant_rot_vel_FoR This constraint forces a constant rotation velocity of a FoR See ``LagrangeConstraints`` for the description of variables Attributes: FoR_body (int): body number of the "FoR" """ _lc_id = 'constant_rot_vel_FoR' def __init__(self): self.required_parameters = ['FoR_body', 'rot_vel'] self._n_eq = 3
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.rot_vel = MBdict_entry['rot_vel'] self.FoR_body = MBdict_entry['FoR_body'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq Bnh[:3,FoR_dof+3:FoR_dof+6] = np.eye(3) LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh) LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific]) LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.FoR_body].for_vel[3:6] - self.rot_vel) ieq += 3 return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
[docs]@lagrangeconstraint class constant_vel_FoR(BaseLagrangeConstraint): __doc__ = """ constant_vel_FoR This constraint forces a constant velocity of a FoR See ``LagrangeConstraints`` for the description of variables Attributes: FoR_body (int): body number of the "FoR" vel (np.ndarray): 6 components of the desired velocity """ _lc_id = 'constant_vel_FoR' def __init__(self): self.required_parameters = ['FoR_body', 'vel'] self._n_eq = 6
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.vel = MBdict_entry['vel'] self.FoR_body = MBdict_entry['FoR_body'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+6] = np.eye(6) LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh) LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific]) LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.FoR_body].for_vel - self.vel) ieq += 6 return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
@lagrangeconstraint class zero_lin_vel_sine_rot_vel_FoR(BaseLagrangeConstraint): __doc__ = """ zero_lin_vel_sine_rot_vel_FoR Zero linear velocity and sinusoidal rotation velocity FoR See ``LagrangeConstraints`` for the description of variables Attributes: FoR_body (int): body number of the "FoR" vel_amp (float): Rotation velocity amplitude omega (float): Frequency of the sinusoidally-varying rotation velocity xyz (string): Axis with the sine velocity """ _lc_id = 'zero_lin_vel_sine_rot_vel_FoR' def __init__(self): self.required_parameters = ['FoR_body', 'vel_amp', 'omega', 'xyz'] self._n_eq = 6 def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): self.FoR_body = MBdict_entry['FoR_body'] self.vel_amp = MBdict_entry['vel_amp'] self.omega = MBdict_entry['omega'] if MBdict_entry['xyz'] == 'x': self.xyz_index = 0 elif MBdict_entry['xyz'] == 'y': self.xyz_index = 1 elif MBdict_entry['xyz'] == 'z': self.xyz_index = 2 else: raise NotImplementedError("FoR rotation velocity shouldd be parallel to x, y or z") self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq vel = np.zeros((6)) vel[3 + self.xyz_index] = self.vel_amp*np.sin(self.omega*ts*dt) Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+6] = np.eye(6) LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh) LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific]) LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.FoR_body].for_vel - vel) ieq += 6 return def staticpost(self, lc_list, MB_beam, MB_tstep): return def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
[docs]@lagrangeconstraint class lin_vel_node_wrtA(BaseLagrangeConstraint): __doc__ = """ lin_vel_node_wrtA This constraint forces the linear velocity of a node to have a certain value with respect to the A FoR See ``LagrangeConstraints`` for the description of variables Attributes: node_number (int): number of the "node" within its own body body_number (int): body number of the "node" vel (np.ndarray): 6 components of the desired velocity with respect to the A FoR """ _lc_id = 'lin_vel_node_wrtA' def __init__(self): self.required_parameters = ['velocity', 'body_number', 'node_number'] self._n_eq = 3
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.vel = MBdict_entry['velocity'] self.body_number = MBdict_entry['body_number'] self.node_number = MBdict_entry['node_number'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR node_dof = define_node_dof(MB_beam, self.body_number, self.node_number) ieq = self._ieq B[:num_LM_eq_specific, node_dof:node_dof+3] = np.eye(3) LM_K[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * B LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(B) LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(B), Lambda[ieq:ieq + num_LM_eq_specific]) LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.body_number].pos[self.node_number,:] - MB_beam[self.body_number].ini_info.pos[self.node_number,:]) ieq += 3 return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): if len(self.vel.shape) > 1: current_vel = self.vel[ts-1, :] else: current_vel = self.vel num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR node_dof = define_node_dof(MB_beam, self.body_number, self.node_number) ieq = self._ieq Bnh[:num_LM_eq_specific, node_dof:node_dof+3] = np.eye(3) LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh) LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific]) LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.body_number].pos_dot[self.node_number,:] - current_vel) ieq += 3 return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
[docs]@lagrangeconstraint class lin_vel_node_wrtG(BaseLagrangeConstraint): __doc__ = """ lin_vel_node_wrtG This constraint forces the linear velocity of a node to have a certain value with respect to the G FoR See ``LagrangeConstraints`` for the description of variables Attributes: node_number (int): number of the "node" within its own body body_number (int): body number of the "node" vel (np.ndarray): 6 components of the desired velocity with respect to the G FoR """ _lc_id = 'lin_vel_node_wrtG' def __init__(self): self.required_parameters = ['velocity', 'body_number', 'node_number'] self._n_eq = 3
[docs] def get_n_eq(self): return self._n_eq
[docs] def initialise(self, MBdict_entry, ieq, print_info=True): self.vel = MBdict_entry['velocity'] self.body_number = MBdict_entry['body_number'] self.node_number = MBdict_entry['node_number'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) return self._ieq + self._n_eq
[docs] def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR node_dof = define_node_dof(MB_beam, self.body_number, self.node_number) ieq = self._ieq B[:num_LM_eq_specific, node_dof:node_dof+3] = MB_tstep[self.body_number].cga() LM_K[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * B LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(B) LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(B), Lambda[ieq:ieq + num_LM_eq_specific]) LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(np.dot(MB_tstep[self.body_number].cga(), MB_tstep[self.body_number].pos[self.node_number,:]) + MB_tstep[self.body_number].for_pos) LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] -= self.scalingFactor*(np.dot(MB_beam[self.body_number].ini_info.cga(), MB_beam[self.body_number].ini_info.pos[self.node_number,:]) + MB_beam[self.body_number].ini_info.for_pos) ieq += 3 return
[docs] def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot): if len(self.vel.shape) > 1: current_vel = self.vel[ts-1, :] else: current_vel = self.vel num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.body_number) node_dof = define_node_dof(MB_beam, self.body_number, self.node_number) ieq = self._ieq if MB_beam[self.body_number].FoR_movement == 'free': Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+3] = MB_tstep[self.body_number].cga() Bnh[:num_LM_eq_specific, FoR_dof+3:FoR_dof+6] = -np.dot(MB_tstep[self.body_number].cga(), ag.skew(MB_tstep[self.body_number].pos[self.node_number,:])) Bnh[:num_LM_eq_specific, node_dof:node_dof+3] = MB_tstep[self.body_number].cga() LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh) if MB_beam[self.body_number].FoR_movement == 'free': LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific]) LM_C[node_dof:node_dof+3, FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific]) LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += self.scalingFactor*np.dot(ag.skew(MB_tstep[self.body_number].pos[self.node_number,:]), ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific])) LM_K[FoR_dof+3:FoR_dof+6, node_dof:node_dof+3] -= self.scalingFactor*ag.skew(np.dot(MB_tstep[self.body_number].cga().T, Lambda_dot[ieq:ieq + num_LM_eq_specific])) LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific]) LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(np.dot( MB_tstep[self.body_number].cga(), ( MB_tstep[self.body_number].for_vel[0:3] + np.dot(ag.skew(MB_tstep[self.body_number].for_vel[3:6]), MB_tstep[self.body_number].pos[self.node_number,:]) + MB_tstep[self.body_number].pos_dot[self.node_number,:])) - current_vel) ieq += 3 return
[docs] def staticpost(self, lc_list, MB_beam, MB_tstep): return
[docs] def dynamicpost(self, lc_list, MB_beam, MB_tstep): return
################################################################################ # Funtions to interact with this Library ################################################################################ def initialize_constraints(MBdict): index_eq = 0 num_constraints = MBdict['num_constraints'] lc_list = list() # Read the dictionary and create the constraints for iconstraint in range(num_constraints): lc_list.append(lc_from_string(MBdict["constraint_%02d" % iconstraint]['behaviour'])()) MBdict_entry = MBdict["constraint_%02d" % iconstraint] if "penaltyFactor" in MBdict_entry.keys(): if not MBdict_entry['penaltyFactor'] == 0.: raise NotImplementedError("Penalty method not completely implemented for Lagrange Constraints") index_eq = lc_list[-1].initialise(MBdict_entry, index_eq) return lc_list def define_num_LM_eq(lc_list): """ define_num_LM_eq Define the number of equations needed to define the boundary boundary conditions Args: lc_list(): list of all the defined contraints Returns: num_LM_eq(int): number of new equations needed to define the boundary boundary conditions Examples: num_LM_eq = lagrangeconstraints.define_num_LM_eq(lc_list) Notes: """ num_LM_eq = 0 # Compute the number of equations for lc in lc_list: num_LM_eq += lc.get_n_eq() return num_LM_eq def generate_lagrange_matrix(lc_list, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot, dynamic_or_static): """ generate_lagrange_matrix Generates the matrices associated to the Lagrange multipliers boundary conditions Args: lc_list(): list of all the defined contraints MBdict(dict): dictionary with the MultiBody and LagrangeMultipliers information MB_beam(list): list of 'beams' of each of the bodies that form the system MB_tstep(list): list of 'StructTimeStepInfo' of each of the bodies that form the system num_LM_eq(int): number of new equations needed to define the boundary boundary conditions sys_size(int): total number of degrees of freedom of the multibody system dt(float): time step Lambda(np.ndarray): list of Lagrange multipliers values Lambda_dot(np.ndarray): list of the first derivative of the Lagrange multipliers values dynamic_or_static (str): string defining if the computation is dynamic or static Returns: LM_C (np.ndarray): Damping matrix associated to the Lagrange Multipliers equations LM_K (np.ndarray): Stiffness matrix associated to the Lagrange Multipliers equations LM_Q (np.ndarray): Vector of independent terms associated to the Lagrange Multipliers equations """ # Initialize matrices LM_C = np.zeros((sys_size + num_LM_eq,sys_size + num_LM_eq), dtype=ct.c_double, order = 'F') LM_K = np.zeros((sys_size + num_LM_eq,sys_size + num_LM_eq), dtype=ct.c_double, order = 'F') LM_Q = np.zeros((sys_size + num_LM_eq,),dtype=ct.c_double, order = 'F') # Define the matrices associated to the constratints # TODO: Is there a better way to deal with ieq? # ieq = 0 for lc in lc_list: if dynamic_or_static.lower() == "static": lc.staticmat(LM_C=LM_C, LM_K=LM_K, LM_Q=LM_Q, MB_beam=MB_beam, MB_tstep=MB_tstep, ts=ts, num_LM_eq=num_LM_eq, sys_size=sys_size, dt=dt, Lambda=Lambda, Lambda_dot=Lambda_dot) elif dynamic_or_static.lower() == "dynamic": lc.dynamicmat(LM_C=LM_C, LM_K=LM_K, LM_Q=LM_Q, MB_beam=MB_beam, MB_tstep=MB_tstep, ts=ts, num_LM_eq=num_LM_eq, sys_size=sys_size, dt=dt, Lambda=Lambda, Lambda_dot=Lambda_dot) return LM_C, LM_K, LM_Q def postprocess(lc_list, MB_beam, MB_tstep, dynamic_or_static): """ Run the postprocess of all the Lagrange Constraints in the system """ for lc in lc_list: if dynamic_or_static.lower() == "static": lc.staticpost(lc_list = lc_list, MB_beam = MB_beam, MB_tstep = MB_tstep) elif dynamic_or_static.lower() == "dynamic": lc.dynamicpost(lc_list = lc_list, MB_beam = MB_beam, MB_tstep = MB_tstep) return def remove_constraint(MBdict, constraint): """ Removes a constraint from the list. This function is thought to release constraints at some point during a dynamic simulation """ try: del(MBdict[constraint]) MBdict['num_constraints'] -= 1 except KeyError: # The entry did not exist in the dict, pass without substracting 1 to # num_constraints pass ################################################################################ ################################################################################ ################################################################################ print_available_lc()