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