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)
def __init__(self, body_id_i, body_id_j, u_iP=np.zeros(2, dtype=float), node_id_j=None, u_iR=np.zeros(2, dtype=float), 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 = "rigid joint rigid-flexible"
super(JointRigidRigidFlexible, 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 = 3#3 or 4 depending on what type of constraint is used for rotation
# 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]
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_grad = self._parent._parent.bodies[self.body_id_j].e_node(self.q0, self.node_id_j)[2:4]
# vector perpendicular to gradient vector of ANCF flexible body j
if (u_iR == np.zeros(2)).all():
# vector perpendicular to gradient vector
r_iR = Ai_ui_P_vector(self.e_j_grad, np.pi/2.)
R_i = np.zeros(2, dtype=float)
theta_i = q2theta_i(self.q0, self.body_id_i)
self.u_iR = transform_cs.gcs2cm_lcs(r_iR, R=R_i, theta=theta_i)
else:
self.u_iR = u_iR
# list of markers
self.markers = self._create_markers()
# update lists
self._update_lists()
# add spring
dict_spring = extract_from_dictionary_by_string_in_key(self._dict, "spring.")
if bool(dict_spring):
self.spring = self._add_spring(dict_spring)
def contact_geometry(self, q):
"""
Normals and tangens in LCS and GCS of each body
:return:
"""
# contact normals and tangents in LCS
theta = q2theta_i(q, self.body_id_j)
# normal in LCS
n_LCS = gcs2cm_lcs(self.normal, np.zeros(2), theta)
self._n_LCS_list = [n_LCS, -n_LCS]
# tangent in LCS
t_LCS = gcs2cm_lcs(self.tangent, np.zeros(2), theta)
self._t_LCS_list = [t_LCS, -t_LCS]
# for i, (body_id, n_LCS_i, t_LCS_i) in enumerate(zip(self.body_id_list, self._n_LCS_list, self._t_LCS_list)):
# theta = q2theta_i(q, body_id)
# self._n_GCS_list[i] = uP_lcs2gcs(n_LCS_i, theta)
# self._t_GCS_list[i] = uP_lcs2gcs(t_LCS_i, theta)
for i, (body_id, sign) in enumerate(zip(self.body_id_list, [+1., -1.])):
theta = q2theta_i(q, body_id)
self._n_GCS_list[i] = self.normal * sign
self._t_GCS_list[i] = self.tangent * sign
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
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)
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)
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
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
def _contact_geometry_GCS(self, q):
"""
Function calculates distance between centerpoints and indentation based on radius of pin/hole
:param q:
:return distance_obj: distance object of calculated data in GCS
"""
self.u_CP_GCS_list = self._contact_geometry_CP_GCS(q)
print "self.u_CP_GCS_list =", self.u_CP_GCS_list
# calculate distance between axis of both bodies in revolute joint
self._distance_obj = DistanceRevoluteClearanceJoint(self.u_CP_GCS_list[0], self.u_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
# contact is present and actual contact point has to be evaluated
# if _distance >= self._radial_clearance and abs(_delta) >= self.distance_TOL:
# print "contact is present"
# unit vector in direction from one center to enother (pin to hole)
self._n_GCS = self._distance_obj._distance_vector / self._distance_obj._distance
# print "--------------------------------"
# print "step =", self._step
# print "n =", self._n
# if _delta < 0 and abs(_delta) > self.distance_TOL:
# print "body i =", self.u_CP_list_GCS[0]
# print "body j =", self.u_CP_list_GCS[1]
# print "n =", self._n
# create normal list in LCS
self._n_GCS_list = [-self._n_GCS, +self._n_GCS]
self._n_LCS_list = []
for body_id, _normal 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=_normal, theta=_theta)
# append normal to list
self._n_LCS_list.append(_normal_LCS)
# calculate a actual contact point in revolute clearance joint of each body in GCS
# and vector of contact point in LCS
self.u_P_GCS_list = []
self.u_P_LCS_list = []
# plot = False#[False, self.status==2] #False
# if plot:
# print "*************************"
# fig = plt.gcf()
# plt.xlim([0.0, 0.01])
# plt.ylim([0.0, 0.01])
# ax = plt.gca()
# # ax.relim()
# ax.autoscale_view(True,True,True)
# ax.set_aspect('equal')
self.u_P_LCS_list = []
# evaluate actual contact point in LCS of each body and in GCS
for body_id, _u_CP_GCS, _u_CP_LCS, _R0 in zip(self.body_id_list, self.u_CP_GCS_list, self.u_CP_LCS_list, self.R0_list):
# print "body_id =", body_id
# q of a body
q_body = q2q_body(q, body_id)
# contact point in GCS
_u_P_GCS = _u_CP_GCS + _R0 * self._n_GCS
# contact point in body LCS
_u_P_LCS = gcs2cm_lcs(_u_P_GCS, q_body[0:2], q_body[2])
self.u_P_LCS_list.append(_u_P_LCS)
# if plot:
# print "----------------------------------"
# print "body =", self._parent._parent.bodies[body_id]._name
#
# R = q_body[0:2]
# print "q_body[0:2] =", q_body[0:2]
# print "joint center =", _u_CP_LCS
# print "contact point GCS =", _u_P_GCS
# print "contact point LCS =", _u_P_LCS
# _color = np.random.rand(3)
# circle=plt.Circle((_u_CP_LCS[0]+R[0],_u_CP_LCS[1]+R[1]),_R,color=_color, fill=False)
# # center of axis
# ax.plot(_u_CP_LCS[0], _u_CP_LCS[1], "o", color=_color)
# # contact point
# ax.plot(_u_P_LCS[0]+R[0], _u_P_LCS[1]+R[1], "x", color=_color)
# # LCS
# ax.plot(R[0], R[1], "s", color=_color)
# fig.gca().add_artist(circle)
# transform to 32bit float to display in opengl
self.u_P_GCS_list = np.array(self.u_P_GCS_list, dtype="float32")
self.u_P_LCS_list = np.array(self.u_P_LCS_list, dtype="float32")
# self.u_P_GCS = np.array(_u_P_GCS, dtype="float32")
# if self._step_solution_accepted:
# self._u_P_solution_container.append(self.u_P_list_LCS.flatten())
# if plot:
# plt.grid()
# plt.show()
# fig.savefig('testing.png')
return _distance, _delta
