Ejemplo n.º 1
0
    def _contact_geometry_GCS(self, q):
        """
        Function calculates distance between center points os spheres and indentation based on radius of sphere i and j
        Evaluated parameters:
        :param _distance:   distance between centers of sphere i and j
        :param _delta:      penetration depth at contact point
        """
        #    create distance object
        # print "q =", q
        # print "i =", self.body_id_i
        # print "j =", self.body_id_j
        # print "i =", q2R_i(q, self.body_id_i)
        # print "j =", q2R_i(q, self.body_id_j)
        self._distance_obj = Distance(q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j))

        #   distance
        _d = self._distance_obj._distance

        #   delta
        _delta = self._distance_obj._distance - self._distance0

        #   normal in GCS
        _n_GCS = -self._distance_obj._distance_vector / self._distance_obj._distance

        #   tangent in GCS
        _t_GCS = n2t(_n_GCS)

        return _d, _delta, _n_GCS, _t_GCS
Ejemplo n.º 2
0
    def _evaluate_contact_distance(self, q):
        """
        Function evaluates contact distance for this type of contact
        :return:
        """
        #    create distance object
        self._distance_obj = Distance(q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j))

        #    distance value
        _distance = self._distance_obj._distance - self._distance0
        return _distance
Ejemplo n.º 3
0
    def _evaluate_contact_distance(self, q):
        """
        Function evaluates contact distance for this type of contact
        :return:
        """
        #    create distance object
        self._distance_obj = Distance(q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j))

        #    distance value
        _distance = self._distance_obj._distance - self._distance0
        return _distance
Ejemplo n.º 4
0
    def _evaluate_distance_2D(self, q=None):
        """
        Args:
            n0 - free node
            r_jP - start node of edge
            r_jR - end node of edge 
        """
        #   coordinates in GCS
        # print "self.n0 =", self.n0
        # print "q2R_i(q, self.node_body_id) =", q2R_i(q, self.node_body_id)
        # print "q2theta_i(q, self.node_body_id) =", q2theta_i(q, self.node_body_id)
        # print "Ai_ui_P_vector(self.n0, q2theta_i(q, self.node_body_id)) =", Ai_ui_P_vector(self.n0, q2theta_i(q, self.node_body_id))
        #
        theta_edge = q2theta_i(q, self.node_body_id)

        self.r_iP = q2R_i(q, self.node_body_id) + Ai_ui_P_vector(
            self.r_iP[0:2], theta_edge)
        # print "n0 =", n0
        self.r_jP = q2R_i(q, self.edge_body_id) + Ai_ui_P_vector(
            self.r_jP[0:2], theta_edge)

        r_jPn0 = self.r_jP - self.r_jP
        # r_jRn0 = self.n0 - self.r_jR
        #
        # #   angle
        # fi_012 = self.angle(r_jPn0, -self.r_jRr_jP)
        # fi_021 = self.angle(r_jRn0, self.r_jRr_jP)
        r_jRr_jP_e = Ai_ui_P_vector(self.r_jRr_jP_e[0:2], theta_edge)

        #
        self._distance_vector = (np.dot(r_jPn0, r_jRr_jP_e) *
                                 r_jRr_jP_e) - r_jPn0

        #   distance
        self._distance = np.linalg.norm(self._distance_vector, ord=2)

        self.normal_plus_distance = self.get_normal_2D(
        ) - self._distance_vector

        #   check if free node is inside (TF status)
        self._TF1 = np.linalg.norm(self.normal_plus_distance) < np.linalg.norm(
            self.normal)
        self._TF2 = self._distance <= self.max_penetration_depth
        #         self._TF3 = fi_012 <= np.pi/2
        #         self._TF4 = fi_021 <= np.pi/2

        #    distance sign
        if np.sign(np.cross(self.edge, r_jPn0)[2]) < 0:
            self._inside = False
            self._distance_sign = self._distance
        else:
            self._inside = True
            self._distance_sign = -self._distance
Ejemplo n.º 5
0
    def _evaluate_distance_2D(self, q=None):
        """
        Args:
            n0 - free node
            n1 - start node of edge
            n2 - end node of edge 
        """
        #   coordinates in GCS
        # print "self.n0 =", self.n0
        # print "q2R_i(q, self.node_body_id) =", q2R_i(q, self.node_body_id)
        # print "q2theta_i(q, self.node_body_id) =", q2theta_i(q, self.node_body_id)
        # print "Ai_ui_P_vector(self.n0, q2theta_i(q, self.node_body_id)) =", Ai_ui_P_vector(self.n0, q2theta_i(q, self.node_body_id))
        #
        theta_edge = q2theta_i(q, self.node_body_id)
        
        n0 = q2R_i(q, self.node_body_id) + Ai_ui_P_vector(self.n0[0:2], theta_edge)
        # print "n0 =", n0
        n1 = q2R_i(q, self.edge_body_id) + Ai_ui_P_vector(self.n1[0:2], theta_edge)
        
        n1n0 = n1 - n0
        # n2n0 = self.n0 - self.n2
        #
        # #   angle
        # fi_012 = self.angle(n1n0, -self.n2n1)
        # fi_021 = self.angle(n2n0, self.n2n1)
        print "self.n2n1_e =", self.n2n1_e
        n2n1_e = Ai_ui_P_vector(self.n2n1_e[0:2], theta_edge)
        
        #    
        self._distance_vector = (np.dot(n1n0, n2n1_e) * n2n1_e) - n1n0

        #   distance
        self._distance = np.linalg.norm(self._distance_vector, ord=2)
        print "self._distance_vector =", self._distance_vector
        self.normal_plus_distance = self.get_normal_2D() - self._distance_vector

        #   check if free node is inside (TF status)
        self._TF1 = np.linalg.norm(self.normal_plus_distance) < np.linalg.norm(self.normal)
        self._TF2 = self._distance <= self.max_penetration_depth
#         self._TF3 = fi_012 <= np.pi/2
#         self._TF4 = fi_021 <= np.pi/2
        
        #    distance sign
        if np.sign(np.cross(self.edge, n1n0)[2]) < 0:
            self._inside = False
            self._distance_sign = self._distance
        else:
            self._inside = True
            self._distance_sign = -self._distance
Ejemplo n.º 6
0
    def evaluate_rijP(self, q):
        """

        :param q:
        :return:
        """
        return r_ij_P(q2R_i(q, self.body_id_i), q2theta_i(q, self.body_id_i), self.u_iP_LCS, q2R_i(q, self.body_id_j), q2theta_i(q, self.body_id_j), self.u_jP_LCS)
Ejemplo n.º 7
0
    def _update_vtk_data(self, t, q):
        """

        :return:
        """
        #   body coordinates R
        self.R[0:2] = q2R_i(q, self.body_id)
        #   body angles theta
        self.theta[-1] = q2theta_i(q, self.body_id)
        if self.vtk_actor is not None:
            self.evaluate_rCAD()

            self.vtk_actor.SetPosition(self.R)
            self.vtk_actor.SetOrientation(np.rad2deg(self.theta))
            print "test =", self.vtk_actor.GetProperty().GetColor(), self._name

        for i, geom in enumerate(self.geometry_list):
            if hasattr(geom, "vtk_actor"):
                geom.vtk_actor.SetPosition(self.R)
                geom.vtk_actor.SetOrientation(np.rad2deg(self.theta))

        # #   LCS marker
        # self.update_vtk_LCS()

        #   geometry marker
        self.update_vtk_geometry_CS()

        #   update AABB
        if hasattr(self.AABBtree, "vtk_actor"):
            self.AABBtree.update_vtk_data(q)
Ejemplo n.º 8
0
    def _mechanical_energy(self, q):
        """

        """
        #    predefine zero array
        _energy = np.zeros(self.MBD_system.number_of_bodies)

        #   energy of all bodies
        for i, body in enumerate(self.MBD_system.bodies):
            _q = q2R_i(q, body.body_id)
            _dq = np.append(q2dR_i(q, body.body_id), q2dtheta_i(q, body.body_id))

            #   body energy
            # body_energy = 0.5 * (body.mass * (_dq**2) + body.J_zz * (_omega**2)) + (body.mass * self.MBD_system.gravity * _q[1])
            body_energy = body.mechanical_energy(q=_q, dq=_dq, gravity=self.MBD_system.gravity)
            _energy[i] = body_energy

        #   energy of normal contact forces
        _energy_of_contacts = np.zeros(len(self.MBD_system.contacts))
        # for i, contact in enumerate(self.MBD_system.contacts):
        #     _energy_of_contacts[i] = contact.mechanical_energy()

        #   total mechanical energy
        energy = np.sum(_energy) + np.sum(_energy_of_contacts)

        return energy
Ejemplo n.º 9
0
    def update_contact_geometry_GCS(self,
                                    q,
                                    u_iP=None,
                                    u_jP=None,
                                    normal_LCS=None,
                                    u_jR=None,
                                    tangent_LCS=None):
        """
        Method updates - calculates contact geometry coordinates of all 3 vector (free node + edge nodes) from LCS to GCS
        :param q_i: coordinates of a point body
        :param q_j: coordinates of a edge body
        :return:
        """
        # print 'q=', q
        if self.u_iP is None and u_iP is not None:
            self.u_iP = u_iP
        # print 'self.u_iP=', self.u_iP

        if self.u_jP is None and u_jP is not None:
            self.u_jP = u_jP
        # print 'self.u_jP=', self.u_jP

        if self.u_jR is None and u_jR is not None:
            self.u_jR = u_jR
        # print 'self.u_jR=', self.u_jR

        if self.normal_LCS is None and normal_LCS is not None:
            self.normal_LCS = normal_LCS

        if self.tangent_LCS is None and tangent_LCS is not None:
            self.tangent_LCS = tangent_LCS

        #   to GCS
        for u, r, body_id in zip(
            [self.u_iP, self.u_jP, self.u_jR], ["r_iP", "r_jP", "r_jR"],
            [self.body_id_i, self.body_id_j, self.body_id_j]):
            R = q2R_i(q, body_id)
            theta = q2theta_i(q, body_id)

            #   point coordinate in LCS
            # print 'u=',u
            _r = cm_lcs2gcs(u, R, theta)

            #   set object attribute
            setattr(self, r, _r)

        theta = q2theta_i(q, self.body_id_j)

        #   normal in LCS
        self.normal = cm_lcs2gcs(self.normal_LCS, np.zeros(2), theta)

        #   tangent in LCS
        self.tangent = n2t(self.normal)

        #   distance
        self._distance_vector = self._evaluate_distance_vector(
            self.r_iP, self.r_jP)

        #   distance and distance sign
        self._evaluate_distance()
Ejemplo n.º 10
0
    def evaluate_rijP(self, q):
        """

        :param q:
        :return:
        """
        return r_ij_P(q2R_i(q, self.body_id_i), q2theta_i(q, self.body_id_i), self.u_iP_LCS, q2R_i(q, self.body_id_j), q2theta_i(q, self.body_id_j), self.u_jP_LCS)
Ejemplo n.º 11
0
    def _load_q(self):
        """
        Function loads vector q from solution file to MBD system object and its bodies
        """
        _path = self.MBD_system.MBD_folder_abs_path_
        _load_file = QtGui.QFileDialog(self)
        _load_file.setDirectory(_path)

        _supported_types = self.__supported_types_solution()

        #   get file path from open file dialog
        file_path = _load_file.getOpenFileName(parent=self._parent, caption=QString("Load q"), directory=QString(_path), filter=QString(_supported_types))
        file_path.replace("/", "\\")

        #   load file data if file is selected
        if file_path:
            #   read data file and store data to solution data object
            solution_data = SolutionData(_file=file_path, MBD_system=self.MBD_system)

            q = self.MBD_system.q0 = solution_data._q_sol_container

            for body in self.MBD_system.bodies:
                #   q to body
                body.R[0:2] = q2R_i(q, body.body_id)
                #   dq to body
                body.theta[2] = q2theta_i(q, body.body_id) 
Ejemplo n.º 12
0
    def _get_contact_geometry_data(self, q):
        """
        Function calculates a vector - point of contact from global coordinates to local coordinates of each body
        """
        #   evaluate normal for each body in contact
        #   in GCS
        self._n_GCS_list = [+self._n_GCS, -self._n_GCS]

        #   in LCS
        #   list of normals in LCS
        self._n_LCS_list = []
        for body_id, n_i in zip(self.body_id_list, self._n_GCS_list):
            #   normal in LCS
            _theta = q2theta_i(q, body_id)
            _normal_LCS = uP_gcs2lcs(u_P=n_i, theta=_theta)
            #   append normal to list
            self._n_list.append(_normal_LCS)

        # print "self._n_list_GCS =", self._n_list_GCS
        #   evaluate tangent
        #   tangent is calculated from rotation of normal for 90deg in CCW direction
        self._t_GCS = np.dot(A_matrix(np.pi/2), self._n_GCS)
        self._t_GCS_list = [self._t_GCS, -self._t_GCS]

        #   contact point on body j in GCS
        self.r_jP_GCS = q2R_i(q, self.body_id_j) + self.R0_j * self._n_GCS_list[1]

        #   contact point on body i in GCS
        u_iP_LCS = self._distance_obj.contact_point_on_line() + self.u_iP_LCS
        self.r_iP_GCS = u_P_lcs2gcs(u_iP_LCS, q, self. body_id_i)

        #   list of contact point in GCS
        self.u_P_GCS_list = [self.r_iP_GCS, self.r_jP_GCS]
Ejemplo n.º 13
0
    def _contact_geometry_LCS(self, q):
        """
        Function evaluates contact geometry parameters in body LCS based on contact geometry in GCS
        Function evaluates:
        self._n_LCS_list:   normal of contact in body LCS:
        self._t_LCS_list:   tangent of contact in body LCS
        self.u_P_LCS_list:  contact point in body LCS
        """
        #   predefine (empty) list of normals in LCS
        self.u_P_LCS_list = []
        self._n_LCS_list = []

        #    vector of contact point in LCS
        for i, (body_id, n_i, r_P) in enumerate(zip(self.body_id_list, self._n_GCS_list, self.r_P_GCS_list)):
            #    R
            R_i = q2R_i(q, body_id)
            #   theta
            theta_i = q2theta_i(q, body_id)

            #   normal in LCS
            normal_LCS = uP_gcs2lcs(n_i, theta=theta_i)
            #   append normal to list
            self._n_LCS_list.append(normal_LCS)

            #   contact point in body LCS
            u_P = gcs2cm_lcs(r_P, R_i, theta_i)
            #   append to list
            self.u_P_LCS_list.append(u_P)
Ejemplo n.º 14
0
    def _contact_geometry_LCS(self, q):
        """
        Function calculates the contact geometry from GCS to LCS
        Especially uPi, uPj
        :param q:
        :return:
        """
        #   evaluate contact points on each body in body LCS
        for i, (body_id, n_i, R0_i) in enumerate(zip(self.body_id_list, self._n_GCS_list, self.R0_list)):
            #   R
            R_i = q2R_i(q, body_id)

            #   theta
            theta_i = q2theta_i(q, body_id)

            #   normal in LCS
            self._n_LCS_list[i] = uP_gcs2lcs(u_P=-n_i, theta=theta_i)

            #   tangent in LCS
            self._t_LCS_list[i] = n2t(self._n_LCS_list[i])

            #   calculate actual contact point in LCS on a body surface
            self.u_P_LCS_list[i] = R0_i * self._n_LCS_list[i]

            #   calculate contact point on each body in GCS
            self.r_P_GCS_list[i] = R_i + uP_gcs2lcs(self.u_P_LCS_list[i], theta_i)
Ejemplo n.º 15
0
Archivo: body.py Proyecto: ladisk/DyS
    def evaluate_CM(self, q):
        """

        :return:
        """
        self.R[0:2] = q2R_i(q, self.body_id)

        return self.R[0:2]
Ejemplo n.º 16
0
    def _update_vtk_data(self, t, q):
        """
        Update of properties and attributes used by this class
        :return:
        """
        #   body coordinates R
        self.R[0:2] = q2R_i(q, self.body_id)

        self.vtk_actor.SetPosition(self.R)
Ejemplo n.º 17
0
    def update_nodes_and_normals_GCS_in_AABB_2D(self, q):
        """
        Function updates nodes and normals in AABB
        """
        #   body R, theta
        R_i = q2R_i(q, self._parent_body.body_id)
        theta_i = q2theta_i(q, self._parent_body.body_id)

        self.update_nodes_GCS_in_AABB_2D(R_i, theta_i)
        self.update_normals_GCS_in_AABB_2D(theta_i)
Ejemplo n.º 18
0
    def update_nodes_and_normals_GCS_in_AABB_2D(self, q):
        """
        Function updates nodes and normals in AABB
        """
        #   body R, theta
        R_i = q2R_i(q, self._parent_body.body_id)
        theta_i = q2theta_i(q, self._parent_body.body_id)

        self.update_nodes_GCS_in_AABB_2D(R_i, theta_i)
        self.update_normals_GCS_in_AABB_2D(theta_i)
Ejemplo n.º 19
0
    def evaluate_rijP(self, q):
        """

        :param q:
        :return:
        """
        rijP = (q2R_i(q, self.body_id_i) + self.u_iP_LCS
                ) - self._parent._parent.bodies[self.body_id_j].e_node(
                    q, self.node_id_j)[0:2]
        return np.linalg.norm(rijP, ord=2)
Ejemplo n.º 20
0
    def update_vtk_data(self, t, q):
        """

        :return:
        """
        #   absolute nodal coordinates of a beam element
        self.e = q2R_i(q, self.body_id)
        #   body coordinates R
        self.R[0:2] = self.e

        self.vtk_actor.SetPosition(self.R)
Ejemplo n.º 21
0
    def _evaluate_Q_e_point_mass(self, t, q, F):
        """
        Evaluate generalized external force for point mass type of body
        :return:
        """
        #   position of force in GCS
        self.r_P_GCS = q2R_i(q, self.body_id)

        #   generalized external force vector
        Q_e = F

        return Q_e
Ejemplo n.º 22
0
Archivo: motion.py Proyecto: ladisk/DyS
    def evaluate_C(self, q, t=0):
        """
        Function evaluates constraint equations on positions level
        :param q:   vector of MBD system
        :param q:   time
        """
        if q is None:
            q = self._parent._parent.get_q()

        if t is False:
            t = 0

        #   predefine vector
        C = np.zeros(self.C_size)
        # print "C(initial) =", C
        j = 0
        # print "self.__q_names =", self.__q_names
        for i, (q_i, q_name) in enumerate(zip(self.q, self.__q_names)):
            # print "q_i =", q_i
            if q_name == "Rx":
                dC = (q2R_i(q, self.body_id) - q2R_i(self.q0, self.body_id))[0]
            if q_name == "Ry":
                dC = (q2R_i(q, self.body_id) - q2R_i(self.q0, self.body_id))[1]
            if q_name == "theta":
                dC = q2theta_i(q, self.body_id) - q2theta_i(
                    self.q0, self.body_id)

            if q_i is not None:
                _str = getattr(self, q_name)
                #   evaluate expression
                val = eval(_str, {}, {"time": t})
                _C = dC - val
                # print "_C =", _C
                C[j] = _C
                j += 1
        # print "C(final) =", C
        return C
Ejemplo n.º 23
0
    def _get_contact_geometry_data(self, q):
        """
        Function calculates a vector - point of contact from global coordinates to local coordinates of each body
        """
        #   evaluate normal for each body in contact
        #   in GCS
        self._n_list_GCS = [self._n, -self._n]
        #   in LCS
        #   list of normals in LCS
        self._n_list = []
        for body_id, normal in zip(self.body_id_list, self._n_list_GCS):
            _theta = q2theta_i(q, body_id)
            _normal_LCS = uP_gcs2lcs(u_P=normal, theta=_theta)
            #   append normal to list
            self._n_list.append(_normal_LCS)

        # print "self._n_list_GCS =", self._n_list_GCS
        #   evaluate tangent
        #   tangent is calculated from rotation of normal for 90deg in CCW direction
        self._t = np.dot(A_matrix(np.pi/2), self._n)
        self._t_list = [self._t, -self._t]

        #   contact point in GCS
        u_Pj = self.R0_j * self._n_list[1]
        self.u_P_GCS = q2R_i(q, self.body_id_j) + Ai_ui_P_vector(u_Pj, q2theta_i(q, self.body_id_j))
        # print "self.u_P_GCS =", self.u_P_GCS
        #   evaluate contact points on each body in body LCS
        self.u_P_list_LCS = []
        for body_id, uP, _normal in zip(self.body_id_list, self.u_P_list_GCS, self._n_list_GCS):
            uP_lcs = self.u_P_GCS - q2R_i(q, body_id)

            _theta = q2theta_i(q, body_id)
            _u_P = uP_gcs2lcs(u_P=uP_lcs, theta=_theta)

            #   append to contact point u_P vector to list
            self.u_P_list_LCS.append(_u_P)
Ejemplo n.º 24
0
    def _contact_geometry_LCS(self, q):
        """

        :param q:
        :return:
        """
        #   in LCS
        # Checked, the method executed correctly - OK!

        for i, (body_id, rP) in enumerate(zip(self.body_id_list, self.r_P_GCS_list)):
            R = q2R_i(q, body_id)
            theta = q2theta_i(q, body_id)
            uP = gcs2cm_lcs(rP, R, theta)

            self.u_P_LCS_list[i] = uP
Ejemplo n.º 25
0
    def evaluate_C(self, q, t=0):
        """
        Function evaluates constraint equations on positions level
        :param q:   vector of MBD system
        :param q:   time
        """
        if q is None:
            q = self._parent._parent.get_q()

        if t is False:
            t = 0

        #   predefine vector
        C = np.zeros(self.C_size)
        # print "C(initial) =", C
        j = 0
        # print "self.__q_names =", self.__q_names
        for i, (q_i, q_name) in enumerate(zip(self.q, self.__q_names)):
            # print "q_i =", q_i
            if q_name == "Rx":
                dC = (q2R_i(q, self.body_id) - q2R_i(self.q0, self.body_id))[0]
            if q_name == "Ry":
                dC = (q2R_i(q, self.body_id) - q2R_i(self.q0, self.body_id))[1]
            if  q_name == "theta":
                dC = q2theta_i(q, self.body_id) - q2theta_i(self.q0, self.body_id)

            if q_i is not None:
                _str = getattr(self, q_name)
                #   evaluate expression
                val = eval(_str, {}, {"time":t})
                _C = dC - val
                # print "_C =", _C
                C[j] = _C
                j += 1
        # print "C(final) =", C
        return C
    def _slot_frame_flat_surface_GCS(self, q):
        """
        Function updates and calculates position of slot frame in global coordinates - GCS
        :param q:
        :return:
        """
        #   nodes in GCS
        frame_nodes_GCS = q2R_i(q, self.body_id_j) + Ai_ui_P_vector(self.frame_nodes_LCS, q2theta_i(q, self.body_id_j))

        #   normals in GCS
        frame_normals_GCS = Ai_ui_P_vector(np.array(self.n_edge_slot_LCS), q2theta_i(q, self.body_id_j))

        #   tangents in GCS
        frame_tangents_GCS = Ai_ui_P_vector(np.array(self.t_edge_slot_LCS), q2theta_i(q, self.body_id_j))

        return frame_nodes_GCS, frame_normals_GCS, frame_tangents_GCS
Ejemplo n.º 27
0
    def _evaluate_r_ij_P(self, q):
        """

        :param q:
        :return:
        """
        for i, (body_id,
                uP) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
            self.r_P_list[i] = q2R_i(q, body_id) + Ai_ui_P_vector(
                uP, q2theta_i(q, body_id))

        [self.r_P_i, self.r_P_j] = self.r_P_list

        #   distance vector
        r_ij_P = self.r_P_i - self.r_P_j

        return r_ij_P
Ejemplo n.º 28
0
    def _contact_geometry_GCS(self, q):
        """
        Function evaluates contact geometry data in GCS
        :param q:
        :return:
        """
        #   transform contact geometry from LCS to GCS
        #   plane only (center of sphere is already calculated in GCS)
        self._n = uP_lcs2gcs(self.n_i_LCS, q2theta_i(q, self.body_id_i))
        #   list of contact point in GCS on each body
        self.u_P_list_GCS = []
        for u_i_LCS in self.u_i_list_LCS:
            if u_i_LCS is not None:
                u_i = q2R_i(q, self.body_id_i) + uP_lcs2gcs(u_i_LCS, q2theta_i(q, self.body_id_i))
            else:
                u_i = None

            self.u_P_list_GCS.append(u_i)
Ejemplo n.º 29
0
    def contact_points_LCS(self, q):
        """

        :param q:
        :return:
        """
        # print "contact_points_LCS()"
        for i, (body_id, rP, Fn, Ft) in enumerate(
                zip(self.body_id_list, self.r_CP_list, self._Fn_list,
                    self._Ft_list)):
            R_i = q2R_i(q, body_id)
            theta_i = q2theta_i(q, body_id)
            uP = gcs2cm_lcs(rP, R=R_i, theta=theta_i)

            self.u_P_LCS_list[i] = uP

            #   update position vectors of force object at contact point
            Fn._update_u_P(uP)
            Ft._update_r_P(rP)
Ejemplo n.º 30
0
    def _evaluate_contact_distance(self, q):
        """
        Function evaluates contact distance for this type of contact
        :return:
        """
        self._contact_geometry_GCS(q)
        #    create distance object
        # print "q2R_i(q, self.body_id_i) =", q2R_i(q, self.body_id_i)
        # print "q2R_i(q, self.body_id_j) =", q2R_i(q, self.body_id_j)
        #   q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j)
        # self._distance_obj = Distance(q2R_i(q, self.body_id_j), self.u_i_list[0], n2=self.u_i_list[1], normal=self.n_i)
        self._distance_obj = DistanceLineNode(q2R_i(q, self.body_id_j), self.r_P_GCS_list[0], self._n_GCS)
        # pprint(vars(self._distance_obj))
        #    distance value
        _distance = self._distance_obj._distance - self._distance0

        # print "_distance(_evaluate_contact_distance) =", _distance
        # time.sleep(10)
        return _distance
Ejemplo n.º 31
0
    def _slot_frame_flat_surface_GCS(self, q):
        """
        Function updates and calculates position of slot frame in global coordinates - GCS
        :param q:
        :return:
        """
        #   nodes in GCS
        frame_nodes_GCS = q2R_i(q, self.body_id_i) + Ai_ui_P_vector(
            self.frame_nodes_LCS, q2theta_i(q, self.body_id_i))

        #   normals in GCS
        frame_normals_GCS = Ai_ui_P_vector(np.array(self.n_edge_slot_LCS),
                                           q2theta_i(q, self.body_id_i))

        #   tangents in GCS
        frame_tangents_GCS = Ai_ui_P_vector(np.array(self.t_edge_slot_LCS),
                                            q2theta_i(q, self.body_id_i))

        return frame_nodes_GCS, frame_normals_GCS, frame_tangents_GCS
Ejemplo n.º 32
0
    def _evaluate_contact_distance(self, q):
        """
        Function evaluates contact distance for this type of contact
        :return:
        """
        self._contact_geometry_GCS(q)
        #    create distance object
        # print "q2R_i(q, self.body_id_i) =", q2R_i(q, self.body_id_i)
        # print "q2R_i(q, self.body_id_j) =", q2R_i(q, self.body_id_j)
        #   q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j)
        # self._distance_obj = Distance(q2R_i(q, self.body_id_j), self.u_i_list[0], n2=self.u_i_list[1], normal=self.n_i)
        self._distance_obj = DistanceLineNode(q2R_i(q, self.body_id_j), self.u_P_list_GCS[0], self._n)
        # pprint(vars(self._distance_obj))
        #    distance value
        _distance = self._distance_obj._distance - self._distance0

        # print "_distance(_evaluate_contact_distance) =", _distance
        # time.sleep(10)
        return _distance
Ejemplo n.º 33
0
    def _contact_geometry_LCS(self, q):
        """
        Function calculates the contact geometry from GCS to LCS (only once).
        Especially uPi, uPj
        :param q:
        :return:
        """
        #   predefine (empty) list of normals in LCS
        self._n_LCS_list = []

        #   evaluate contact points on each body in body LCS
        self.u_P_LCS_list = []
        for body_id, r_P, _normal in zip(self.body_id_list, self.u_P_GCS_list, self._n_GCS_list):
            # uP_lcs = self.u_P_GCS - q2R_i(q, body_id)

            # _theta = q2theta_i(q, body_id)
            # _u_P = uP_gcs2lcs(u_P=uP_lcs, theta=_theta)
            u_P = gcs2cm_lcs(r_P, q2R_i(q, body_id), q2theta_i(q, body_id))
            #   append to contact point u_P vector to list
            self.u_P_LCS_list.append(u_P)
Ejemplo n.º 34
0
    def _contact_geometry_GCS(self, q):
        """
        Function evaluates contact geometry data in GCS
        :param q:
        :return:
        """
        self.r_i_list_GCS, self.r_jP_GCS = self._contact_geometry_P_GCS(q)

        #   plane normal in GCS
        self._n_GCS = Ai_ui_P_vector(self.n_i_LCS, q2theta_i(q, self.body_id_i))

        #   distance object
        self._distance_obj = DistanceLineNode(q2R_i(q, self.body_id_j), self.r_P_GCS_list[0], normal=self._n_GCS)

        #   get distance and delta value
        _distance = self._distance_obj._distance

        _delta = _distance - self.R0_j

        return _distance, _delta
Ejemplo n.º 35
0
    def update_contact_points_GCS(self, q):
        """
        Function updates coordinates of contact points (geometry) in GCS based on LCS coordinates and vector q
        :param q: vector of MBD system coordinates
        :return:
        """
        # print "update_contact_points_GCS@", __file__
        for i, (body_id, uP) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
            R = q2R_i(q, body_id)
            theta = q2theta_i(q, body_id)

            #   point coordinate in LCS
            rP = cm_lcs2gcs(uP, R, theta)

            self.r_P_GCS_list[i] = rP

        #   distance and delta are equal and multiplied with (-1)
        self._distance_vector = self._evaluate_distance_vector(self.r_P_GCS_list[0], self.r_P_GCS_list[1])

        # _distance = self._distance_sign = - np.linalg.norm(self._distance_vector, ord=2)
        _distance = self._distance_sign = - np.linalg.norm((self.normal * np.dot(self._distance_vector, self.normal)), ord=2)
        return _distance, self._distance_sign, self.normal, self.tangent
Ejemplo n.º 36
0
    def __init__(self,
                 body_id_i,
                 body_id_j,
                 u_iP=np.array([0, 0], dtype=float),
                 node_id_j=None,
                 properties_dict={},
                 parent=None):
        """
        :param support:
        :param body_id_i:   id of rigid body
        :param body_id_j:   id of flexible body
        :param u_iP_CAD:
        :param u_jP_CAD:
        :param u_iQ:
        :param parent:
        :return:
        """
        self.joint_type = "revolute joint rigid-flexible"
        super(JointRevoluteRigidFlexible,
              self).__init__(self.joint_type,
                             body_id_i,
                             body_id_j,
                             u_iP_CAD=u_iP,
                             properties_dict=properties_dict,
                             parent=parent)

        #   number of constrained nodal coordinates per node of finite element
        self.n_CNC = 2

        #   body type list
        self.body_type_list = ["rigid", "flexible"]

        #   size of vector q for body i and j
        self.e_n_i = GlobalVariables.q_i_dim[self.body_id_i]
        self.e_n_j = GlobalVariables.q_i_dim[self.body_id_j]
        self.e_n_list = [3, self.e_n_j]

        #   C_q matrix dimensions [rows, cols]
        #   number of columns is different for rigid and flexible body
        self.C_q_dim = [self.n_CNC, self.e_n_list]
        self.C_q_i = None
        self.C_q_j = None

        #   node id (node k of body j)
        self.node_id_j = node_id_j

        self.e_j_grad = q2e_x_jk(self.q0, self.body_id_j, self.node_id_j)
        self.e_j = q2e_jk(self.q0, self.body_id_j, self.node_id_j)

        #   list of markers
        self.markers = self._create_markers()

        #   update lists
        self._update_lists()

        #   number of nodal coordinates of a flexible body (mesh)
        self.e_n = self._parent._parent.bodies[self.body_id_j].mesh.n_NC

        #   vector perpendicular to gradient vector of ANCF flexible body j
        if (u_iP == np.zeros(2)).all():
            #   vector perpendicular to gradient vector
            r_iP = self.e_j

            R_i = q2R_i(self.q0, self.body_id_i)
            theta_i = q2theta_i(self.q0, self.body_id_i)
            self.u_iP = transform_cs.gcs2cm_lcs(r_iP, R=R_i, theta=theta_i)

        else:
            self.u_iP = u_iP
Ejemplo n.º 37
0
    def __init__(self,
                 body_id_i,
                 body_id_j,
                 u_iP=np.zeros(2, dtype=float),
                 node_id_j=None,
                 properties_dict={},
                 parent=None):
        """
        :param support:
        :param body_id_i:   id of point mass
        :param body_id_j:   id of flexible body
        :param u_iP_CAD:
        :param u_jP_CAD:
        :param u_iQ:
        :param parent:
        :return:
        """
        self.joint_type = "rigid joint point mass-flexible"
        super(JointRigidPointMassFlexible,
              self).__init__(self.joint_type,
                             body_id_i,
                             body_id_j,
                             u_iP_CAD=u_iP,
                             properties_dict=properties_dict,
                             parent=parent)

        #   number of constrained nodal coordinates per node of finite element
        self.n_CNC = 2

        #   body type list
        self.body_type_list = ["point mass", "flexible"]

        #   size of vector q for body i and j
        self.e_n_i = GlobalVariables.q_i_dim[self.body_id_i]
        self.e_n_j = GlobalVariables.q_i_dim[self.body_id_j]
        self.e_n_list = [self.e_n_i, self.e_n_j]

        #   C_q matrix dimensions [rows, cols]
        self.C_q_dim = [self.n_CNC, self.e_n_list]
        self.C_q_i = None
        self.C_q_j = None

        #   node id (node k of body j)
        self.node_id_j = node_id_j

        #   gradient vector on flexible body j
        self.e_j = self._parent._parent.bodies[self.body_id_j].e_node(
            self.q0, self.node_id_j)[0:2]
        self.e_j_grad = self._parent._parent.bodies[self.body_id_j].e_node(
            self.q0, self.node_id_j)[2:4]

        #   vector of joint position in point mass coordinate system

        if (u_iP == np.zeros(2)).all():
            u_iP = self.e_j - q2R_i(self.q0, self.body_id_i)

        self.u_iP_LCS = self.u_P_LCS_list[0] = self.u_iP = u_iP

        #   list of markers
        self.markers, self.r_P_GCS_list = self._create_markers()

        #   update lists
        self._update_lists()
Ejemplo n.º 38
0
    def __init__(self, joint_type, body_id_i, body_id_j, u_iP_CAD=np.array([0, 0]), u_jP_CAD=np.array([0, 0]), u_iQ_CAD=np.array([0, 0]), properties_dict={}, parent=None):
        """
        Create a joint object
        :param joint_type:      type of 2D joint (revolute, prismatic, fixed) as string
        :param body_id_i:       id of body i
        :param body_id_j:       id of body j
        :param u_iP_CAD:        in CAD LCS of a body i
        :param u_jP_CAD:        in CAD LCS of a body j
        :param u_iQ_CAD:        in CAD LCS of a body i (only for prismatic joint)
        :param parent:
        """
        super(Joint, self).__init__(joint_type, parent)
        #    number
        self.joint_id = self.__id.next()  # len(joints_list) + 1
        self._parent = parent

        if self._parent is not None:
            self.joints_list = self._parent._parent.joints
        else:
            self.joints_list = []

        #   joint type
        self.joint_type = joint_type

        #   dictionary of joint properties
        self._dict = properties_dict

        #   boolean for constant C_q matrix of joint
        self.constant = False

        #   visualization (opengl) properties
        self.z_dim = 0.
        #   get q vector of current MBD system at joint object initialization
        self.q0 = self._parent._parent.evaluate_q0()

        #   spring at joint coordinates
        self.spring = None


        if self.joint_type not in self._supported_types:
            raise ValueError, "Joint type not correct!"

        #   body ids
        self.body_id_i = body_id_i
        self.body_id_j = body_id_j
        #   check ids
        if self.joint_type != "slope discontinuity":
            self._check_ids(self.body_id_i, self.body_id_j, uiP=u_iP_CAD, ujP=u_jP_CAD)

        #   body id list
        self.body_id_list = [self.body_id_i, self.body_id_j]
        self.body_list = []

        #   body type list
        self.body_type_list = ["rigid", "rigid"]

        #   flexible body data if a flexible body is in joint
        self.node_id_i = None
        self.node_id_j = None
        self.node_id_list = [self.node_id_i, self.node_id_j]

        self.element_id_i = None
        self.element_id_j = None
        self.element_id_list = [self.element_id_i, self.element_id_j]

        self.element_ksi_i = None
        self.element_ksi_j = None
        self.element_ksi_list = [self.element_ksi_i, self.element_ksi_j]

        #   list of point vectors to joint constraint in CAD lCS of a body
        self.u_P_CAD_list = [u_iP_CAD, u_jP_CAD]

        #   predefined empty list to store point vectors of joint constraint in LCS (center of gravity)
        #   of each body
        self.u_P_LCS_list = []
        self.r_P_GCS_list = [None, None]

        self.u_iQ_CAD = u_iQ_CAD

        #   predefined variable of vector C(q, t) constraint equation
        self.C0 = None
        self.theta0 = None

        #   time dependent attributes and values
        self.C = None

        # if self.body_id_i == "ground" or self.body_id_j == "ground":
        #     _body_id = copy(self.body_id_list)
        #     _body_id.remove("ground")
        #     _body_id = _body_id[0]
        #
        #     indx = self.body_id_list.index(_body_id)
        #     C1 = q2R_i(self.q0, _body_id) + Ai_ui_P_vector(self.u_P_CAD_list[indx], q2theta_i(self.q0, _body_id))
        #
        #     C2 = q2theta_i(self.q0, self.body_id_list[indx])
        #
        #     self.C0 = np.append(C1, C2)
        # else:
        #     self.C0 = np.zeros(3)

        #   predefined empty list
        self.C_q_list = []
        self.Q_d_list = []

        #   list of joint points
        #   1   uPi - body i
        #   2   uQi - body i
        #   3   uPj - body j
        self.u_QP_LCS_list = []
        
        #    vector of lagrange multipliers at simulation time
        self.L = None
        #    list of vectors of constraint reaction forces for each body in joint
        self.Q_c_list = [None, None]

        #   pair of contact force list
        self.contact_bodies_added_to_list = False
        self.list_of_contact_force_objects_constructed = False
        self.force_list = []

        #   calculate u_P vector of every body in body LCS
        for i, (body_id, _u_P) in enumerate(zip(self.body_id_list, self.u_P_CAD_list)):
            if body_id == "ground" or body_id == -1 or body_id is None:
                u_P_LCS = _u_P
                rP = None
                _body = self._parent._parent.ground
            else:
                #   pointer to body object
                _body = self._parent._parent.bodies[body_id]
                if _body.body_type == "rigid body":
                    #   calculate point vector in body LCS (center of gravity)
                    u_P_LCS = u_P_cad2cm_lcs(body_id, _body, _u_P, 0)#_body.theta[2]

                    R_i = q2R_i(self.q0, body_id)
                    theta_i = q2theta_i(self.q0, body_id)
                    rP = cm_lcs2gcs(u_P_LCS, R=R_i, theta=theta_i)
                else:
                    u_P_LCS = None
                    rP = None

            self.u_P_LCS_list.append(u_P_LCS)
            self.r_P_GCS_list[i] = rP

            #   list of body pointers
            self.body_list.append(_body)

        [self.u_iP_LCS, self.u_jP_LCS] = self.u_P_LCS_list

        #   solution options:
        #   Discard (default)
        #   Overwrite (existing file)
        #   Save to new (next available) file
        self._solution_save_options = "discard"
        #   solution file type, optional: .dat, .xlsx, .csv
        self._solution_filetype = ".xlsx"
        
        #   markers
        self.markers = []

        #   create forces
        self._create_Fn_forces()
        
        #   init solution container
        self._solution_containers()
        
        #    add additional properties from dictionary
        self.add_attributes_from_dict(properties_dict)

        #   visualization properties
        self.vtk_actor = None
    def _contact_geometry_LCS(self, q):
        """
        Function evaluates contact geometry parameters in body LCS based on contact geometry in GCS
        Function evaluates:
        self._n_LCS_list:   normal of contact in body LCS:
        self._t_LCS_list:   tangent of contact in body LCS
        self.u_P_LCS_list:  contact point in body LCS
        """
        # print "_contact_geometry_LCS()"
        if self.pin_in_section_jPjR:
            #   contact point on body i - pin
            #   in LCS
            self.u_P_LCS_list[0] = self.u_iP_LCS = Ai_ui_P_vector(-self._n_GCS * self.R0_i, q2theta_i(q, self.body_id_i))
            #   in GCS
            self.u_P_GCS_list[0] = self.u_iP_GCS = q2R_i(q, self.body_id_i) + self.u_iP_LCS

            #   contact point on body j in GCS
            self.u_P_GCS = self.u_P_GCS_list[1] = self.u_jP_GCS = self._distance_obj.contact_point_on_line_GCS()
            # time.sleep(10)
            #   contact point on body j in LCS
            self.u_P_LCS_list[1] = self.u_jP_LCS = gcs2cm_lcs(self.u_P_GCS, q2R_i(q, self.body_id_j), q2theta_i(q, self.body_id_j))

            #   in GCS
            # self.u_iP_GCS = u_P_lcs2gcs(self.u_iP_LCS, q, self.body_id_i) + (-self._n_GCS) * self.R0_i
            # print "self.u_iP_GCS =", self.u_iP_GCS

            #   get normal and tangent of each body
            for i, (sign, body_id) in enumerate(zip([-1, +1], self.body_id_list)):
                theta = q2theta_i(q, body_id)
                #   evaluate body normal in body LCS
                self._n_LCS_list[i] = gcs2cm_lcs(sign * self._n_GCS, theta=theta)
                #   evaluate body tangent in body LCS
                self._t_LCS_list[i] = gcs2cm_lcs(sign * self._t_GCS, theta=theta)

        elif self.pin_in_section_jP or self.pin_in_section_jR:
            #   create normal list in LCS
            self._n_GCS_list = [self._n_GCS, -self._n_GCS]
            for i, (body_id, _normal) in enumerate(zip(self.body_id_list, self._n_GCS_list)):
                #   normal in LCS
                _theta = q2theta_i(q, body_id)
                # _normal_LCS = Ai_ui_P_vector(_normal, _theta)
                _normal_LCS = uP_gcs2lcs(_normal, _theta)
                #   append normal to list
                self._n_LCS_list[i] = _normal_LCS

            if self.pin_in_section_jP:
                _u_CP_LCS_list = [self.u_CP_LCS_list[0], self.u_CP_LCS_list[1]]
                _u_CP_GCS_list = [self.u_CP_GCS_list[0], self.u_CP_GCS_list[1]]

            if self.pin_in_section_jR:
                _u_CP_LCS_list = [self.u_CP_LCS_list[0], self.u_CP_LCS_list[2]]
                _u_CP_GCS_list = [self.u_CP_GCS_list[0], self.u_CP_GCS_list[2]]

            #   evaluate actual contact point in LCS of each body and in GCS
            for i, (body_id, _u_CP_LCS, _R0) in enumerate(zip(self.body_id_list, _u_CP_LCS_list, self.R0_list)):
                #   R of body
                R = q2R_i(q, body_id)
                #   theta of body
                theta = q2theta_i(q, body_id)

                #   contact point in body LCS
                _u_P_LCS = _u_CP_LCS
                self.u_P_LCS_list[i] = _u_P_LCS# + _R0 * self._n_GCS
                #   contact point in GCS
                self.u_P_GCS_list[i] = R + Ai_ui_P_vector(_u_CP_LCS, theta) + _R0 * self._n_GCS