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 _create_markers(self):
"""
Function creates markers
:return:
"""
markers = []
# create markers for joint centers
for i, u_P in enumerate(self.u_CP_LCS_list):
# get correct body object
body_id = body = None
if i == 0 or i == 1:
body = self.body_list[0]
body_id = self.body_id_list[0]
if i == 2:
body = self.body_list[1]
body_id = self.body_id_list[1]
# node coordinates in GCS
rP = u_P_lcs2gcs(u_P, self.q0, body_id)
# add 3rd dimension for visualization
rP = np.array(np.append(rP, self.z_dim), dtype="float32")
if body is not None:
# create marker object
marker = Marker(rP, parent=self)
# append marker to list of contact markers
markers.append(marker)
return markers
def update_F(self,
q,
step=None,
F=np.zeros(2),
u_P=np.zeros(2),
r_P=np.zeros(2)):
"""
:param step:
:param Fx:
:param Fy:
:param Mz:
:return:
"""
if step is None:
pass
else:
self._step_num_solution_container.append(step)
# update applied force vector
self._update_F(F)
# update position vector of applied force
# LCS
if (u_P == np.zeros(2)).all():
u_P = self.u_iP_f
self._update_u_P(u_P)
# GCS
if (r_P == np.zeros(2)).all() and hasattr(q, "__len__"):
r_P = u_P_lcs2gcs(u_P, q, self.body_id)
self._update_r_P(r_P)
def reset(self, q):
"""
Function defined in subclass
:return:
"""
for i, (body_id, uP, node_id) in enumerate(
zip(self.body_id_list, self.u_P_LCS_list, self.node_id_list)):
self.r_P_list[i] = u_P_lcs2gcs(uP, q, body_id, node_id=node_id)
def _pin_GCS(self, q):
"""
Function updates and calculates position of a pin center in global coordinates - GCS
:param q:
:return:
"""
CP_GCS = u_P_lcs2gcs(self.u_jP, q, self.body_id_j)
return CP_GCS
def _contact_geometry_P_GCS(self, q):
"""
Function evaluates position of body points (sphere center) and points that define edge - line
:return:
"""
r_i_list_GCS = []
for i, body_id in enumerate(self.body_id_list):
# geometry points of line
if i == 0:
for u_iP in self.u_i_list_LCS:
if u_iP is not None:
r_iP = u_P_lcs2gcs(u_iP, q, body_id)
r_i_list_GCS.append(r_iP)
# circle center
if i == 1:
r_jP_GCS = u_P_lcs2gcs(self.u_jP_LCS, q, body_id)
return r_i_list_GCS, r_jP_GCS
def _pin_GCS(self, q):
"""
Function updates and calculates position of a pin center in global coordinates - GCS
:param q:
:return:
"""
# CP_GCS = q2R_i(q, self.body_id_i) + Ai_ui_P_vector(self.u_iP, q2theta_i(q, self.body_id_i))
CP_GCS = u_P_lcs2gcs(self.u_iP, q, self.body_id_i)
return CP_GCS
def evaluate_C(self, q, t=0.):
"""
Function is defined in subclass
:param q:
:param t:
:return:
"""
self.C = np.zeros(self.n_CNC)
# node on rigid body in GCS
rP_i = u_P_lcs2gcs(
self.u_P_LCS_list[self.body_id_list.index(self.body_id_i)], q,
self.body_id_i)
# node on flexible body
rP_j = self._parent._parent.bodies[self.body_id_j].evaluate_r(
q, node_id=self.node_id_j)
# list of coordinates vectors
self.r_P_GCS_list = [rP_i, rP_j]
# vector C
self.C[0:2] = rP_i - rP_j
# vector perpendicular to gradient vector
# theta_i = q2theta_i(q, self.body_id_i)
# rR_i = Ai_ui_P_vector(self.u_iR, theta_i)
# gradient vector at node of a joint
# self.e_j_grad = self._parent._parent.bodies[self.body_id_j].e_node(q, self.node_id_j)[2:4]
# self.e_j_grad = q2e_x_jk(q, self.body_id_j, self.node_id_j)
# e_j_grad_unit = e_j_grad / np.linalg.norm(e_j_grad)
# scalar product of two vectors
# rR_i_scaled = rR_i * np.linalg.norm(self.e_j_grad)
# print "rR_i =", rR_i
# print "self.e_j_grad =", self.e_j_grad
# print "np.dot() =", np.dot(rR_i, self.e_j_grad)
# print "fi =", np.rad2deg(np.arccos(np.dot(rR_i, self.e_j_grad) / (np.linalg.norm(rR_i) * np.linalg.norm(self.e_j_grad))))
# plt.plot([0, rR_i[0]], [0, rR_i[1]], color="blue")
# plt.plot([0, self.e_j_grad[0]], [0, self.e_j_grad[1]], color="green")
# plt.show()
# C[2] = np.dot(rR_i, e_j_grad)
# vector C
# self.C[2] = np.dot(rR_i, self.e_j_grad)
# override for constraint check - this equations is not explicitly used at each integration time step
# self.C[2] = 0.
# C[2] = rR_i_scaled - self.e_j_grad
# print "test =", np.dot(self.e_j_grad, Ai_ui_P_vector(self.u_iR, theta_i + np.deg2rad(90.)))
# print "rR_i =", rR_i, "e_j_grad =", e_j_grad, "C[2:4] =", C[2:4], "rR_i - e_j_grad =", rR_i - e_j_grad, "dfi =", np.arccos(np.dot(rR_i, e_j_grad) / (np.linalg.norm(rR_i) * np.linalg.norm(e_j_grad))), np.linalg.norm(rR_i), np.linalg.norm(e_j_grad)
# print "dfi =", np.rad2deg(np.arccos(np.dot(rR_i, e_j_grad_unit) / (np.linalg.norm(rR_i) * np.linalg.norm(e_j_grad_unit))))
return self.C
def _slot_GCS(self, q):
"""
:param q:
:return:
"""
slot_CP_GCS = []
for CP in self.u_CP_slot_LCS:
_uP_gcs = u_P_lcs2gcs(CP, q, self.body_id_i)
slot_CP_GCS.append(_uP_gcs)
return slot_CP_GCS
def evaluate_rijP(self, q):
"""
Length of spring at time when vector of absolute coordinates is equal to q
"""
for i, (body_id,
uP) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
self.r_P_list[i] = u_P_lcs2gcs(uP, q, body_id)
r_ij_P = self.r_P_list[0] - self.r_P_list[1]
return r_ij_P
def _slot_GCS(self, q):
"""
:param q:
:return:
"""
slot_CP_GCS = []
for CP in self.u_CP_slot_LCS:
_uP_gcs = u_P_lcs2gcs(CP, q, self.body_id_j)
slot_CP_GCS.append(_uP_gcs)
return slot_CP_GCS
def _contact_geometry_CP_GCS(self, q):
"""
Function calculates position of centers (CP - Center Points) of revolute joint pin/hole in GCS
:param q: a vector of coordinates of the system (numpy array)
:return u_CP_list_GCS: a list of two arrays (vectors) of
"""
# calculate position of pin/hole centers of each body in GCS
for i, (body_id, u_P) in enumerate(zip(self.body_id_list, self.u_CP_LCS_list)):
# axis center of revolute joint of each body in GCS
self.r_CP_GCS_list[i] = u_P_lcs2gcs(u_P, q, body_id)
# convert to 32bit float to display in opengl scene
return np.array(self.r_CP_GCS_list, dtype="float32")
def _reset(self, q):
"""
:return:
"""
# node on rigid body in GCS
rP_i = u_P_lcs2gcs(self.u_P_LCS_list[self.body_id_list.index(self.body_id_i)], q, self.body_id_i)
# node on flexible body
rP_j = self._parent._parent.bodies[self.body_id_j].evaluate_r(q, node_id=self.node_id_j)
# list of coordinates vectors
self.r_P_GCS_list = [rP_i, rP_j]
def _contact_geometry_CP_GCS(self, q):
"""
Function calculates position of centers (CP - Center Points) of revolute joint pin/hole in GCS
:param q: a vector of coordinates of the system (numpy array)
:return u_CP_list_GCS: a list of two arrays (vectors) of
"""
# calculate position of pin/hole centers of each body in GCS
u_CP_GCS_list = []
print "q =", q
for body_id, u_P in zip(self.body_id_list, self.u_CP_LCS_list):
print "u_P =", u_P
# axis center of revolute joint of each body in GCS
u_P_GCS = u_P_lcs2gcs(u_P, q, body_id)
print "u_P_GCS =", u_P_GCS
u_CP_GCS_list.append(u_P_GCS)
# reformat to 32bit float to display in opengl scene
return np.array(u_CP_GCS_list, dtype="float32")
def _evaluate_Q_e_rigid(self, t, q, F):
"""
Evaluate generalized external force for rigid body
:param t:
:param q:
:return:
"""
# position of force in GCS
self.r_P_GCS = u_P_lcs2gcs(self.u_P_LCS, q, self.body_id)
# get theta of body from q vector
theta = q2theta_i(q, self.body_id)
# construct a matrix (function of vector q)
matrix = Force_Q_e_matrix(self.body_id, self.u_iP_f, theta)
# form a vector
Q_e = np.dot(matrix.matrix, F)
return Q_e
def reset(self, q0):
"""
:return:
"""
# set q0
self.q0 = q0
self.Fx = 0.
self.Fy = 0.
self.Mz = 0.
if self._body.body_type == "rigid body":
self.r_P_GCS = u_P_lcs2gcs(self.u_P_LCS, self.q0, self.body_id)
if self._body.body_type == "flexible body":
self.r_P_GCS = self.q0
self.F = self._evaluate_F(0.)
self.update_vtk_data()
def _create_markers(self):
"""
Function create markers
:return:
"""
markers = []
self.r_P_list = []
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
if body_id == "ground" or body_id == -1:
# pointer to body object
body = self._parent._parent.ground
r_P = u_P
else:
r_P = u_P_lcs2gcs(u_P, self.q0, body_id)
# pointer to body object
if self._parent is not None:
body = self._parent._parent.bodies[body_id]
else:
body = None
self.r_P_list.append(r_P)
if body is not None:
# create marker object
r_node = np.array(np.append(r_P, self.z_dim), dtype="float32")
u_node = np.array(np.append(u_P, self.z_dim), dtype="float32")
marker = Marker(r_node, uP=u_node, body_id=body, parent=body)
# append marker object
# to body list of markers
body.markers.append(marker)
# to markers list
markers.append(marker)
self.body_list.append(body)
return markers
def evaluate_C(self, q, t=None):
"""
Function evaluates C for the joint
:param q:
:param t:
:return:
"""
# node on rigid body in GCS
rP_i = u_P_lcs2gcs(
self.u_P_LCS_list[self.body_id_list.index(self.body_id_i)], q,
self.body_id_i)
# node on flexible body
rP_j = self._parent._parent.bodies[self.body_id_j].evaluate_r(
q, node_id=self.node_id_j)
# list of coordinates vectors
self.r_P_GCS_list = [rP_i, rP_j]
# vector C
self.C = rP_i - rP_j
return self.C
