Example #1
0
    def _contact_velocity(self, q):
        """
        Function evaluates relative contact velocity vectors in normal and tangent direction
        """
        dr_P = []
        for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
            #   body velocity, R, theta
            _dR = q2dR_i(q, body_id)
            _theta = q2theta_i(q, body_id)

            #    dtheta - omega
            _dtheta = q2dtheta_i(q, body_id)

            #    velocity at contact point (in GCS) on a body
            _dr_P = dr_contact_point_uP(_dR, _theta, _dtheta, u_P)
            
            #    appned to list
            dr_P.append(_dr_P)
        
        #    relative contact velocity vector
        _dq = dr_P[1] - dr_P[0]
        
        #   relative contact velocity (scalar value)
        #   normal direction
        _dq_n = np.dot(_dq, self._n_GCS)

        #   tangent direction
        _dq_t = np.dot(_dq, self._t_GCS)
        return _dq_n, _dq_t
Example #2
0
    def _contact_velocity(self, q):
        """
        Function evaluates relative contact velocity vectors in normal and tangent direction
        """
        dr_P = []
        for body_id, _R, _n, _t in zip(self.body_id_list, self.R0_list, self._n_list, self._t_list):
            #   contact point in body LCS
            _uP = _R * _n

            #   body velocity, R, theta
            _dR = q2dR_i(q, body_id)
            _theta = q2theta_i(q, body_id)

            #    dtheta - omega
            _dtheta = q2dtheta_i(q, body_id)

            #    velocity at contact point (in GCS) on a body
            _dr_P = dr_contact_point_uP(_dR, _theta, _dtheta, _uP)

            #    appned to list
            dr_P.append(_dr_P)

        #    relative contact velocity vector
        _dq = dr_P[1] - dr_P[0]

        #   relative contact velocity (scalar value)
        #   normal direction
        _dq_n = np.dot(_dq, self._n)
        #   tangent direction
        _dq_t = np.dot(_dq, self._t)
        return _dq_n, _dq_t
    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
Example #4
0
    def contact_velocity(self, q):
        """
        positive value - bodies are approaching
        negative values - bodies are moving apart
        :return:
        """
        dr_P = [np.zeros(2, dtype="float32"), np.zeros(2, dtype="float32")]

        for i, (body_id,
                u_P) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
            #   body velocity, R, theta
            dR = q2dR_i(q, body_id)
            theta = q2theta_i(q, body_id)
            #    dtheta - omega
            dtheta = q2dtheta_i(q, body_id)
            #    point velocity
            dr_P_i = dr_contact_point_uP(dR, theta, dtheta, u_P)
            #    update list
            dr_P[i] = dr_P_i

        #    relative contact velocity vector
        _dq = dr_P[1] - dr_P[0]

        #   relative contact velocity
        #   normal direction
        self._n_GCS = self.n_list[self.index]
        self._dq_n = np.dot(_dq, self._n_GCS)

        #   tangent direction
        self._t_GCS = self.t_list[self.index]
        self._dq_t = np.dot(_dq, self._t_GCS)

        return self._dq_n, self._dq_t
Example #5
0
    def contact_velocity(self, q):
        """
        Function evaluates relative contact velocity vectors in normal and tangent direction
        :param q:
        :return:
        """
        dr_P = [np.zeros(2, dtype="float32"), np.zeros(2, dtype="float32")]

        for i, (body_id,
                u_P) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
            #   body velocity, R, theta
            dR = q2dR_i(q, body_id)
            theta = q2theta_i(q, body_id)
            #    dtheta - omega
            dtheta = q2dtheta_i(q, body_id)
            #    point velocity
            dr_P_i = dr_contact_point_uP(dR, theta, dtheta, u_P)
            #    update list
            dr_P[i] = dr_P_i

        #    relative contact velocity vector
        _dq = dr_P[1] - dr_P[0]

        #   relative contact velocity
        #   normal direction
        self._n_GCS = self.normal
        self._dq_n = np.dot(_dq, self._n_GCS)

        #   tangent direction
        self._t_GCS = self.tangent
        self._dq_t = np.dot(_dq, self._t_GCS)

        return self._dq_n, self._dq_t
    def _contact_velocity(self, q):
        """
        Function evaluates relative contact velocity at contact point in revolute clearance joint.
        :param q:   array of coordinates (r, theta) and velocities (dR, dhete=omega) of MBD system
        """
        dr_P = []
        print "self.u_P_LCS_list =", self.u_P_LCS_list, type(self.u_P_LCS_list)
        for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
            dR = q2dR_i(q, body_id)
            theta = q2theta_i(q, body_id)
            #    dtheta - omega
            dtheta = q2dtheta_i(q, body_id)

            #    point velocity
            dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)

            #    add to list
            dr_P.append(dr_P_body)

        _dq = dr_P[0] - dr_P[1]

        #   relative contact velocity
        #   normal direction
        _dq_n = np.dot(_dq, self._n_GCS)

        #   tangent direction
        _dq_t = np.dot(_dq, self._t_GCS)

        return _dq_n, _dq_t
Example #7
0
    def _contact_velocity(self, q):
        """

        Args:
            q:

        Returns:

        """
        dr_P = []
        for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
            #   body velocity, R, theta
            dR = q2dR_i(q, body_id)
            theta = q2theta_i(q, body_id)

            #    dtheta - omega
            dtheta = q2dtheta_i(q, body_id)

            #    point velocity
            dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)
            # print 'dr_P_body=', dr_P_body, 'body_id=', body_id

            #    add to list
            dr_P.append(dr_P_body)

        _dq = dr_P[1] - dr_P[0]

        self._dq_n = np.dot(_dq, self.normal)
        # print 'dq_n=', self._dq_n, 'normal=', self.normal, _dq
        self._dq_t = np.dot(_dq, self.tangent)

        return self._dq_n, self._dq_t
Example #8
0
File: body.py Project: ladisk/DyS
    def evaluate_kinetic_energy(self, q):
        """

        :return:
        """
        #   vector of translational velocity
        if q is None:
            dR = self.dR[0:2]
        else:
            if len(q) == self.q_i_size:
                dR = q[0:2]
            else:
                dR = q2dR_i(q, self.body_id)

        #   float of angular velocity
        if q is None:
            dtheta = self.dtheta[2]
        else:
            if len(q) == self.q_i_size:
                dtheta = q[2]
            else:
                dtheta = q2dtheta_i(q, self.body_id)

        #   kinetic energy
        self._kinetic_energy = 0.5 * self.mass * (np.linalg.norm(dR)**2) + .5 * self.J_zz * (dtheta**2)
        return self._kinetic_energy
Example #9
0
    def _contact_velocity(self, q):
        """
        Function evaluates relative contact velocity at contact point in revolute clearance joint.
        :param q:   array of coordinates (r, theta) and velocities (dR, dheta=omega) of MBD system
        """
        dr_P = []
        for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
            dR = q2dR_i(q, body_id)
            theta = q2theta_i(q, body_id)
            #    dtheta - omega
            dtheta = q2dtheta_i(q, body_id)

            #    point velocity
            dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)

            #    add to list
            dr_P.append(dr_P_body)

        _dq = dr_P[1] - dr_P[0]

        #   relative contact velocity
        #   normal direction
        _dq_n = np.dot(_dq, self._n_GCS)

        #   tangent direction
        _dq_t = np.dot(_dq, self._t_GCS)

        return _dq_n, _dq_t
Example #10
0
    def _evaluate_dl(self, q, I_r_ij_P):
        """

        :return:
        """
        #   vector of dq_i, dq_j
        dq = np.array([np.append(q2dR_i(q, self.body_id_i), q2dtheta_i(q, self.body_id_i)),
                       np.append(q2dR_i(q, self.body_id_j), q2dtheta_i(q, self.body_id_j))]).flatten()

        #    dr_ij_P_dq
        dr_ij_P_dq_ = dr_ij_P_dq(body_id_i=self.body_id_i,
                                 theta_i=q2theta_i(q, self.body_id_i),
                                 u_iP=self.u_iP_LCS,
                                 body_id_j=self.body_id_j,
                                 theta_j=q2theta_i(q, self.body_id_j),
                                 u_jP=self.u_jP_LCS)

        #    velocity of deformation of spring length
        dl = np.dot(I_r_ij_P, np.dot(dr_ij_P_dq_, dq))

        return dl
Example #11
0
    def evaluate_Q_d(self, q):
        """
        Function evaluates Q_d vector of a joint
        Q_d equation according to eq. 3.133 (example 3.14) in Computational Dynamics 3rd Ed. (A.A.Shabana)
        Args:
            bodies - list of all bodies
            q - array of ppositions and velosities
            N - number of bodies
        Returns:
            Q_d - vector Q_d for joint
        """
        Q_d = np.zeros(2)

        #   get theta i, j
        theta_i = q2theta_i(q, self.body_id_i)
        theta_j = q2theta_i(q, self.body_id_j)

        #   get translational absolute velocity of body - R i, j
        dR_i = q2dR_i(q, self.body_id_i)
        dR_j = q2dR_i(q, self.body_id_j)

        #   get rotational absolute velocity of body i, j
        dtheta_i = q2dtheta_i(q, self.body_id_i)
        dtheta_j = q2dtheta_i(q, self.body_id_j)

        #   evaluate h_i
        hi = self.evaluate_hi(q)
        #   evaluate distance between
        rijP = self.evaluate_rijP(q)

        Q_d_21 = 2 * theta_i * np.dot(np.dot(A_theta_matrix(theta_i), hi), (dR_i - dR_j))
        Q_d_22 = (dtheta_i ** 2) * (np.dot(self.u_iP_LCS, self.u_iQ_LCS) - np.dot(rijP, hi))
        Q_d_23 = 2 * dtheta_i * dtheta_j * np.dot(np.dot(A_theta_matrix(theta_i), hi), np.dot(A_theta_matrix(theta_j), self.u_jP_LCS))
        Q_d_24 = (dtheta_j ** 2) * np.dot(hi, self.u_jP_LCS)
        Q_d[0] = -Q_d_21 - Q_d_22 + Q_d_23 - Q_d_24

        return Q_d
Example #12
0
    def evaluate_Q_d(self, q):
        """
        Function evaluates Q_d vector of a joint
        Q_d equation according to eq. 3.133 (example 3.14) in Computational Dynamics 3rd Ed. (A.A.Shabana)
        Args:
            bodies - list of all bodies
            q - array of ppositions and velosities
            N - number of bodies
        Returns:
            Q_d - vector Q_d for joint
        """
        Q_d = np.zeros(2)

        #   get theta i, j
        theta_i = q2theta_i(q, self.body_id_i)
        theta_j = q2theta_i(q, self.body_id_j)

        #   get translational absolute velocity of body - R i, j
        dR_i = q2dR_i(q, self.body_id_i)
        dR_j = q2dR_i(q, self.body_id_j)

        #   get rotational absolute velocity of body i, j
        dtheta_i = q2dtheta_i(q, self.body_id_i)
        dtheta_j = q2dtheta_i(q, self.body_id_j)

        #   evaluate h_i
        hi = self.evaluate_hi(q)
        #   evaluate distance between
        rijP = self.evaluate_rijP(q)

        Q_d_21 = 2 * theta_i * np.dot(np.dot(A_theta_matrix(theta_i), hi), (dR_i - dR_j))
        Q_d_22 = (dtheta_i ** 2) * (np.dot(self.u_iP_LCS, self.u_iQ_LCS) - np.dot(rijP, hi))
        Q_d_23 = 2 * dtheta_i * dtheta_j * np.dot(np.dot(A_theta_matrix(theta_i), hi), np.dot(A_theta_matrix(theta_j), self.u_jP_LCS))
        Q_d_24 = (dtheta_j ** 2) * np.dot(hi, self.u_jP_LCS)
        Q_d[0] = -Q_d_21 - Q_d_22 + Q_d_23 - Q_d_24

        return Q_d