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 _get_contact_geometry_data(self, q):
"""
Function calculates a vector - point of contact from global coordinates to local coordinates of each body
"""
# evaluate normal
self._n = self._distance_obj.get_normal_2D()
# print "self._n =", self._n
self._n_list_GCS = [self._n, -self._n]
# evaluate tangent
# tangent is calculated from rotation of normal for 90deg in CCW direction
self._t = np.dot(A_matrix(np.pi / 2), self._n)
self._t_list = []
# evaluate contact points on each body in body LCS
self._n_list = []
self.u_P_list_LCS = []
for body_id, _normal, _R0 in zip(self.body_id_list, self._n_list_GCS, self.R0_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_list.append(_normal_LCS)
# tangent in LCS
_tangent_LCS = np.dot(A_matrix(np.pi / 2), _normal)
self._t_list.append(_tangent_LCS)
# calculate actual contact point in LCS on a body surface
_u_P = _R0 * _normal_LCS
# append to contact point u_P vector to list
self.u_P_list_LCS.append(_u_P)
def _get_contact_geometry_data(self, q):
"""
Function calculates a vector - point of contact from global coordinates to local coordinates of each body
"""
# evaluate normal for each body in contact
# in GCS
self._n_GCS_list = [+self._n_GCS, -self._n_GCS]
# in LCS
# list of normals in LCS
self._n_LCS_list = []
for body_id, n_i 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=n_i, theta=_theta)
# append normal to list
self._n_list.append(_normal_LCS)
# print "self._n_list_GCS =", self._n_list_GCS
# evaluate tangent
# tangent is calculated from rotation of normal for 90deg in CCW direction
self._t_GCS = np.dot(A_matrix(np.pi/2), self._n_GCS)
self._t_GCS_list = [self._t_GCS, -self._t_GCS]
# contact point on body j in GCS
self.r_jP_GCS = q2R_i(q, self.body_id_j) + self.R0_j * self._n_GCS_list[1]
# contact point on body i in GCS
u_iP_LCS = self._distance_obj.contact_point_on_line() + self.u_iP_LCS
self.r_iP_GCS = u_P_lcs2gcs(u_iP_LCS, q, self. body_id_i)
# list of contact point in GCS
self.u_P_GCS_list = [self.r_iP_GCS, self.r_jP_GCS]
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 _get_contact_geometry_data(self, q):
"""
Function calculates a vector - point of contact from global coordinates to local coordinates of each body
"""
# evaluate normal for each body in contact
# in GCS
self._n_list_GCS = [self._n, -self._n]
# in LCS
# list of normals in LCS
self._n_list = []
for body_id, normal in zip(self.body_id_list, self._n_list_GCS):
_theta = q2theta_i(q, body_id)
_normal_LCS = uP_gcs2lcs(u_P=normal, theta=_theta)
# append normal to list
self._n_list.append(_normal_LCS)
# print "self._n_list_GCS =", self._n_list_GCS
# evaluate tangent
# tangent is calculated from rotation of normal for 90deg in CCW direction
self._t = np.dot(A_matrix(np.pi/2), self._n)
self._t_list = [self._t, -self._t]
# contact point in GCS
u_Pj = self.R0_j * self._n_list[1]
self.u_P_GCS = q2R_i(q, self.body_id_j) + Ai_ui_P_vector(u_Pj, q2theta_i(q, self.body_id_j))
# print "self.u_P_GCS =", self.u_P_GCS
# evaluate contact points on each body in body LCS
self.u_P_list_LCS = []
for body_id, uP, _normal in zip(self.body_id_list, self.u_P_list_GCS, self._n_list_GCS):
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)
# append to contact point u_P vector to list
self.u_P_list_LCS.append(_u_P)
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
