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
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
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
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
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
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
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
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
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
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