def __init__(self, r_iP, r_jP, parent=None):
"""
Constructor of a distance object
:param u_iP: center of body i hole in GCS coordinates
_param u_jP: center of body j pin in GCS coordinates
:return:
"""
# parent
self._parent = parent
# body ids
# i - free point of pin body id
# j - line body of slot body id
self.body_id_i = self._parent.body_id_i
self.body_id_j = self._parent.body_id_j
self.body_id_list = [self.body_id_i, self.body_id_j]
# points in GCS
self.r_jP = r_jP
self.r_iP = r_iP
self._distance_vector = self.r_jP - self.r_iP
self._distance = np.linalg.norm(self._distance_vector, ord=2)
# normal in GCS of cylindrical surface
self.normal = self._distance_vector / self._distance
# tangent in GCS
self.tangent = n2t(self.normal)
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()
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)
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
def _get_contact_geometry_data(self, q):
"""
Function locates and creates contact point vector in GCS based on position of both bodies in contact
:param q:
:return:
"""
# print "_get_contact_geometry_data()"
if self.pin_in_section_jPjR:
# contact point on body j - slot
# GCS
self.u_P_GCS_list[1] = self.u_jP_GCS = self._distance_obj.contact_point_on_line_GCS()
# LCS
self.u_jP_LCS = u_P_gcs2lcs(self.u_jP_GCS, q, self.body_id_j)
# print "self.u_jP_LCS =", self.u_jP_LCS
# get normal in GCS
self._n_GCS = self._distance_obj.normal
# print "self._n_GCS =", self._n_GCS
elif self.pin_in_section_jP or self.pin_in_section_jR:
# get normal in GCS
self._n_GCS = self._distance_obj._distance_vector / self._distance_obj._distance
else:
raise ValueError, "Contact in slot section not detected!"
# get tangent in CS
self._t_GCS = n2t(self._n_GCS)
self._contact_point_found = True
def _contact_geometry_GCS(self, q):
"""
Function calculates distance between center points (eccentricity vector) and indentation based on radius of pin/hole
Function calculates actual contact point on each body in body LCS and GCS
:param q: vector of states of MBD system
:return _distance: distance between center of pin and hole
:paramm _delta: depth of deformation (indentation, penetration)
:return _n_GCS: normal of contact in GCS
_return _t_GCS: tangent of contact in GCS
"""
# a list of center point of pin and hole on each body in GCS
self.r_CP_GCS_list = self._contact_geometry_CP_GCS(q)
# calculate distance between axis of both bodies in revolute joint
self._distance_obj = DistanceRCJ(self.r_CP_GCS_list[0], self.r_CP_GCS_list[1], parent=self)
# penetration depth is difference between nominal radial clearance and actual calculated clearance at time t
distance = self._distance_obj._distance
delta = self._radial_clearance - distance
# unit vector in direction from one center to another (pin to hole)
n_GCS = self._distance_obj._distance_vector / self._distance_obj._distance
# normal unit vector for body i, j
n_GCS_list = [n_GCS, -n_GCS]
# tangent in GCS
t_GCS = n2t(n_GCS)
# calculate a actual contact point in revolute clearance joint of each body in GCS
self.r_P_GCS_list = copy.copy(self.r_P_GCS_0_list)
# evaluate actual contact point in LCS of each body and in GCS
for i, (_u_CP_GCS, _u_CP_LCS, _R0, n_i) in enumerate(zip(self.r_CP_GCS_list, self.u_CP_LCS_list, self.R0_list, n_GCS_list)):
# contact point in GCS
_u_P_GCS = _u_CP_GCS + _R0 * n_i
# add to list
self.r_P_GCS_list[i] = _u_P_GCS
return distance, delta, n_GCS, t_GCS
def __init__(self,
r_iP,
r_jP,
r_jR=None,
normal=None,
q=None,
node_body=None,
edge_body=None,
parent=None):
"""
Constructor of a distance object that calculates distance (value, scalar) of free node to edge
:param n0: free node of a body in body LCS
:param r_jP: start node of an edge of a body in body LCS
:param r_jR: end node of an edge of a body in body LCS
:param normal: normal of an edge in body LCS
:param q: vector of generalized coordinates at time t
:param node_body: pointer to node body object
:param edge_body:
"""
# parent
self._parent = parent
# default value for attribute
self._distance = np.inf
# initialize data containers
self._data_container()
# node on body i - free node
self.r_iP = r_iP
# node on body j
self.r_jP = r_jP
# normal
self.normal = normal
# tangent
if self.normal is not None:
self.tangent = n2t(self.normal)
else:
self.tangent = None
if node_body is not None and edge_body is not None:
# pointers to edge and node body
self.node_body = node_body
self.node_body_id = node_body.body_id
self.edge_body = edge_body
self.edge_body_id = edge_body.body_id
# edge vector from 1 to 2
self.r_jRr_jP = r_jR - r_jP
# print "self.edge =", self.r_jRr_jP
# angle = np.arccos(np.dot(r_jP, r_jR))
# print "angle =", angle
# edge nodes
self.r_jP = r_jP
self.r_jR = r_jR
# edge vector
self.edge = r_jR - r_jP
# evaluate direction of edge vector it has to be in direction of normal
self._check_direction(r_jP, r_jR)
# unit edge vector
self.r_jRr_jP_e = self.edge / np.linalg.norm(self.edge)
# max penetration depth attribute assigned from body attribute
if edge_body is not None:
self.max_penetration_depth = edge_body.max_penetration_depth
else:
self.max_penetration_depth = 1E-4
self.min_penetration_depth = 10 * self.max_penetration_depth
# evaluate distance
# evaluate distance: node to node
if r_jR is None:
self._distance, self._inside = self._vector_length(
self.r_iP, self.r_jP)
# evaluate distance: node to edge
else:
self._evaluate_distance_2D(q)
# calculate tangent based on normal
self.tangent = Ai_ui_P_vector(self.normal, np.pi / 2)