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 _evaluate_contact_distance(self, q):
"""
Function evaluates contact distance for this type of contact
:return:
"""
# create distance object
self._distance_obj = Distance(q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j))
# distance value
_distance = self._distance_obj._distance - self._distance0
return _distance
def _evaluate_contact_distance(self, q):
"""
Function evaluates contact distance for this type of contact
:return:
"""
# create distance object
self._distance_obj = Distance(q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j))
# distance value
_distance = self._distance_obj._distance - self._distance0
return _distance
def _evaluate_distance_2D(self, q=None):
"""
Args:
n0 - free node
r_jP - start node of edge
r_jR - end node of edge
"""
# coordinates in GCS
# print "self.n0 =", self.n0
# print "q2R_i(q, self.node_body_id) =", q2R_i(q, self.node_body_id)
# print "q2theta_i(q, self.node_body_id) =", q2theta_i(q, self.node_body_id)
# print "Ai_ui_P_vector(self.n0, q2theta_i(q, self.node_body_id)) =", Ai_ui_P_vector(self.n0, q2theta_i(q, self.node_body_id))
#
theta_edge = q2theta_i(q, self.node_body_id)
self.r_iP = q2R_i(q, self.node_body_id) + Ai_ui_P_vector(
self.r_iP[0:2], theta_edge)
# print "n0 =", n0
self.r_jP = q2R_i(q, self.edge_body_id) + Ai_ui_P_vector(
self.r_jP[0:2], theta_edge)
r_jPn0 = self.r_jP - self.r_jP
# r_jRn0 = self.n0 - self.r_jR
#
# # angle
# fi_012 = self.angle(r_jPn0, -self.r_jRr_jP)
# fi_021 = self.angle(r_jRn0, self.r_jRr_jP)
r_jRr_jP_e = Ai_ui_P_vector(self.r_jRr_jP_e[0:2], theta_edge)
#
self._distance_vector = (np.dot(r_jPn0, r_jRr_jP_e) *
r_jRr_jP_e) - r_jPn0
# distance
self._distance = np.linalg.norm(self._distance_vector, ord=2)
self.normal_plus_distance = self.get_normal_2D(
) - self._distance_vector
# check if free node is inside (TF status)
self._TF1 = np.linalg.norm(self.normal_plus_distance) < np.linalg.norm(
self.normal)
self._TF2 = self._distance <= self.max_penetration_depth
# self._TF3 = fi_012 <= np.pi/2
# self._TF4 = fi_021 <= np.pi/2
# distance sign
if np.sign(np.cross(self.edge, r_jPn0)[2]) < 0:
self._inside = False
self._distance_sign = self._distance
else:
self._inside = True
self._distance_sign = -self._distance
def _evaluate_distance_2D(self, q=None):
"""
Args:
n0 - free node
n1 - start node of edge
n2 - end node of edge
"""
# coordinates in GCS
# print "self.n0 =", self.n0
# print "q2R_i(q, self.node_body_id) =", q2R_i(q, self.node_body_id)
# print "q2theta_i(q, self.node_body_id) =", q2theta_i(q, self.node_body_id)
# print "Ai_ui_P_vector(self.n0, q2theta_i(q, self.node_body_id)) =", Ai_ui_P_vector(self.n0, q2theta_i(q, self.node_body_id))
#
theta_edge = q2theta_i(q, self.node_body_id)
n0 = q2R_i(q, self.node_body_id) + Ai_ui_P_vector(self.n0[0:2], theta_edge)
# print "n0 =", n0
n1 = q2R_i(q, self.edge_body_id) + Ai_ui_P_vector(self.n1[0:2], theta_edge)
n1n0 = n1 - n0
# n2n0 = self.n0 - self.n2
#
# # angle
# fi_012 = self.angle(n1n0, -self.n2n1)
# fi_021 = self.angle(n2n0, self.n2n1)
print "self.n2n1_e =", self.n2n1_e
n2n1_e = Ai_ui_P_vector(self.n2n1_e[0:2], theta_edge)
#
self._distance_vector = (np.dot(n1n0, n2n1_e) * n2n1_e) - n1n0
# distance
self._distance = np.linalg.norm(self._distance_vector, ord=2)
print "self._distance_vector =", self._distance_vector
self.normal_plus_distance = self.get_normal_2D() - self._distance_vector
# check if free node is inside (TF status)
self._TF1 = np.linalg.norm(self.normal_plus_distance) < np.linalg.norm(self.normal)
self._TF2 = self._distance <= self.max_penetration_depth
# self._TF3 = fi_012 <= np.pi/2
# self._TF4 = fi_021 <= np.pi/2
# distance sign
if np.sign(np.cross(self.edge, n1n0)[2]) < 0:
self._inside = False
self._distance_sign = self._distance
else:
self._inside = True
self._distance_sign = -self._distance
def evaluate_rijP(self, q):
"""
:param q:
:return:
"""
return r_ij_P(q2R_i(q, self.body_id_i), q2theta_i(q, self.body_id_i), self.u_iP_LCS, q2R_i(q, self.body_id_j), q2theta_i(q, self.body_id_j), self.u_jP_LCS)
def _update_vtk_data(self, t, q):
"""
:return:
"""
# body coordinates R
self.R[0:2] = q2R_i(q, self.body_id)
# body angles theta
self.theta[-1] = q2theta_i(q, self.body_id)
if self.vtk_actor is not None:
self.evaluate_rCAD()
self.vtk_actor.SetPosition(self.R)
self.vtk_actor.SetOrientation(np.rad2deg(self.theta))
print "test =", self.vtk_actor.GetProperty().GetColor(), self._name
for i, geom in enumerate(self.geometry_list):
if hasattr(geom, "vtk_actor"):
geom.vtk_actor.SetPosition(self.R)
geom.vtk_actor.SetOrientation(np.rad2deg(self.theta))
# # LCS marker
# self.update_vtk_LCS()
# geometry marker
self.update_vtk_geometry_CS()
# update AABB
if hasattr(self.AABBtree, "vtk_actor"):
self.AABBtree.update_vtk_data(q)
def _mechanical_energy(self, q):
"""
"""
# predefine zero array
_energy = np.zeros(self.MBD_system.number_of_bodies)
# energy of all bodies
for i, body in enumerate(self.MBD_system.bodies):
_q = q2R_i(q, body.body_id)
_dq = np.append(q2dR_i(q, body.body_id), q2dtheta_i(q, body.body_id))
# body energy
# body_energy = 0.5 * (body.mass * (_dq**2) + body.J_zz * (_omega**2)) + (body.mass * self.MBD_system.gravity * _q[1])
body_energy = body.mechanical_energy(q=_q, dq=_dq, gravity=self.MBD_system.gravity)
_energy[i] = body_energy
# energy of normal contact forces
_energy_of_contacts = np.zeros(len(self.MBD_system.contacts))
# for i, contact in enumerate(self.MBD_system.contacts):
# _energy_of_contacts[i] = contact.mechanical_energy()
# total mechanical energy
energy = np.sum(_energy) + np.sum(_energy_of_contacts)
return energy
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 evaluate_rijP(self, q):
"""
:param q:
:return:
"""
return r_ij_P(q2R_i(q, self.body_id_i), q2theta_i(q, self.body_id_i), self.u_iP_LCS, q2R_i(q, self.body_id_j), q2theta_i(q, self.body_id_j), self.u_jP_LCS)
def _load_q(self):
"""
Function loads vector q from solution file to MBD system object and its bodies
"""
_path = self.MBD_system.MBD_folder_abs_path_
_load_file = QtGui.QFileDialog(self)
_load_file.setDirectory(_path)
_supported_types = self.__supported_types_solution()
# get file path from open file dialog
file_path = _load_file.getOpenFileName(parent=self._parent, caption=QString("Load q"), directory=QString(_path), filter=QString(_supported_types))
file_path.replace("/", "\\")
# load file data if file is selected
if file_path:
# read data file and store data to solution data object
solution_data = SolutionData(_file=file_path, MBD_system=self.MBD_system)
q = self.MBD_system.q0 = solution_data._q_sol_container
for body in self.MBD_system.bodies:
# q to body
body.R[0:2] = q2R_i(q, body.body_id)
# dq to body
body.theta[2] = q2theta_i(q, body.body_id)
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 _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 evaluate_CM(self, q):
"""
:return:
"""
self.R[0:2] = q2R_i(q, self.body_id)
return self.R[0:2]
def _update_vtk_data(self, t, q):
"""
Update of properties and attributes used by this class
:return:
"""
# body coordinates R
self.R[0:2] = q2R_i(q, self.body_id)
self.vtk_actor.SetPosition(self.R)
def update_nodes_and_normals_GCS_in_AABB_2D(self, q):
"""
Function updates nodes and normals in AABB
"""
# body R, theta
R_i = q2R_i(q, self._parent_body.body_id)
theta_i = q2theta_i(q, self._parent_body.body_id)
self.update_nodes_GCS_in_AABB_2D(R_i, theta_i)
self.update_normals_GCS_in_AABB_2D(theta_i)
def update_nodes_and_normals_GCS_in_AABB_2D(self, q):
"""
Function updates nodes and normals in AABB
"""
# body R, theta
R_i = q2R_i(q, self._parent_body.body_id)
theta_i = q2theta_i(q, self._parent_body.body_id)
self.update_nodes_GCS_in_AABB_2D(R_i, theta_i)
self.update_normals_GCS_in_AABB_2D(theta_i)
def evaluate_rijP(self, q):
"""
:param q:
:return:
"""
rijP = (q2R_i(q, self.body_id_i) + self.u_iP_LCS
) - self._parent._parent.bodies[self.body_id_j].e_node(
q, self.node_id_j)[0:2]
return np.linalg.norm(rijP, ord=2)
def update_vtk_data(self, t, q):
"""
:return:
"""
# absolute nodal coordinates of a beam element
self.e = q2R_i(q, self.body_id)
# body coordinates R
self.R[0:2] = self.e
self.vtk_actor.SetPosition(self.R)
def _evaluate_Q_e_point_mass(self, t, q, F):
"""
Evaluate generalized external force for point mass type of body
:return:
"""
# position of force in GCS
self.r_P_GCS = q2R_i(q, self.body_id)
# generalized external force vector
Q_e = F
return Q_e
def evaluate_C(self, q, t=0):
"""
Function evaluates constraint equations on positions level
:param q: vector of MBD system
:param q: time
"""
if q is None:
q = self._parent._parent.get_q()
if t is False:
t = 0
# predefine vector
C = np.zeros(self.C_size)
# print "C(initial) =", C
j = 0
# print "self.__q_names =", self.__q_names
for i, (q_i, q_name) in enumerate(zip(self.q, self.__q_names)):
# print "q_i =", q_i
if q_name == "Rx":
dC = (q2R_i(q, self.body_id) - q2R_i(self.q0, self.body_id))[0]
if q_name == "Ry":
dC = (q2R_i(q, self.body_id) - q2R_i(self.q0, self.body_id))[1]
if q_name == "theta":
dC = q2theta_i(q, self.body_id) - q2theta_i(
self.q0, self.body_id)
if q_i is not None:
_str = getattr(self, q_name)
# evaluate expression
val = eval(_str, {}, {"time": t})
_C = dC - val
# print "_C =", _C
C[j] = _C
j += 1
# print "C(final) =", C
return C
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):
"""
: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 evaluate_C(self, q, t=0):
"""
Function evaluates constraint equations on positions level
:param q: vector of MBD system
:param q: time
"""
if q is None:
q = self._parent._parent.get_q()
if t is False:
t = 0
# predefine vector
C = np.zeros(self.C_size)
# print "C(initial) =", C
j = 0
# print "self.__q_names =", self.__q_names
for i, (q_i, q_name) in enumerate(zip(self.q, self.__q_names)):
# print "q_i =", q_i
if q_name == "Rx":
dC = (q2R_i(q, self.body_id) - q2R_i(self.q0, self.body_id))[0]
if q_name == "Ry":
dC = (q2R_i(q, self.body_id) - q2R_i(self.q0, self.body_id))[1]
if q_name == "theta":
dC = q2theta_i(q, self.body_id) - q2theta_i(self.q0, self.body_id)
if q_i is not None:
_str = getattr(self, q_name)
# evaluate expression
val = eval(_str, {}, {"time":t})
_C = dC - val
# print "_C =", _C
C[j] = _C
j += 1
# print "C(final) =", C
return C
def _slot_frame_flat_surface_GCS(self, q):
"""
Function updates and calculates position of slot frame in global coordinates - GCS
:param q:
:return:
"""
# nodes in GCS
frame_nodes_GCS = q2R_i(q, self.body_id_j) + Ai_ui_P_vector(self.frame_nodes_LCS, q2theta_i(q, self.body_id_j))
# normals in GCS
frame_normals_GCS = Ai_ui_P_vector(np.array(self.n_edge_slot_LCS), q2theta_i(q, self.body_id_j))
# tangents in GCS
frame_tangents_GCS = Ai_ui_P_vector(np.array(self.t_edge_slot_LCS), q2theta_i(q, self.body_id_j))
return frame_nodes_GCS, frame_normals_GCS, frame_tangents_GCS
def _evaluate_r_ij_P(self, q):
"""
:param q:
:return:
"""
for i, (body_id,
uP) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
self.r_P_list[i] = q2R_i(q, body_id) + Ai_ui_P_vector(
uP, q2theta_i(q, body_id))
[self.r_P_i, self.r_P_j] = self.r_P_list
# distance vector
r_ij_P = self.r_P_i - self.r_P_j
return r_ij_P
def _contact_geometry_GCS(self, q):
"""
Function evaluates contact geometry data in GCS
:param q:
:return:
"""
# transform contact geometry from LCS to GCS
# plane only (center of sphere is already calculated in GCS)
self._n = uP_lcs2gcs(self.n_i_LCS, q2theta_i(q, self.body_id_i))
# list of contact point in GCS on each body
self.u_P_list_GCS = []
for u_i_LCS in self.u_i_list_LCS:
if u_i_LCS is not None:
u_i = q2R_i(q, self.body_id_i) + uP_lcs2gcs(u_i_LCS, q2theta_i(q, self.body_id_i))
else:
u_i = None
self.u_P_list_GCS.append(u_i)
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 _evaluate_contact_distance(self, q):
"""
Function evaluates contact distance for this type of contact
:return:
"""
self._contact_geometry_GCS(q)
# create distance object
# print "q2R_i(q, self.body_id_i) =", q2R_i(q, self.body_id_i)
# print "q2R_i(q, self.body_id_j) =", q2R_i(q, self.body_id_j)
# q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j)
# self._distance_obj = Distance(q2R_i(q, self.body_id_j), self.u_i_list[0], n2=self.u_i_list[1], normal=self.n_i)
self._distance_obj = DistanceLineNode(q2R_i(q, self.body_id_j), self.r_P_GCS_list[0], self._n_GCS)
# pprint(vars(self._distance_obj))
# distance value
_distance = self._distance_obj._distance - self._distance0
# print "_distance(_evaluate_contact_distance) =", _distance
# time.sleep(10)
return _distance
def _slot_frame_flat_surface_GCS(self, q):
"""
Function updates and calculates position of slot frame in global coordinates - GCS
:param q:
:return:
"""
# nodes in GCS
frame_nodes_GCS = q2R_i(q, self.body_id_i) + Ai_ui_P_vector(
self.frame_nodes_LCS, q2theta_i(q, self.body_id_i))
# normals in GCS
frame_normals_GCS = Ai_ui_P_vector(np.array(self.n_edge_slot_LCS),
q2theta_i(q, self.body_id_i))
# tangents in GCS
frame_tangents_GCS = Ai_ui_P_vector(np.array(self.t_edge_slot_LCS),
q2theta_i(q, self.body_id_i))
return frame_nodes_GCS, frame_normals_GCS, frame_tangents_GCS
def _evaluate_contact_distance(self, q):
"""
Function evaluates contact distance for this type of contact
:return:
"""
self._contact_geometry_GCS(q)
# create distance object
# print "q2R_i(q, self.body_id_i) =", q2R_i(q, self.body_id_i)
# print "q2R_i(q, self.body_id_j) =", q2R_i(q, self.body_id_j)
# q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j)
# self._distance_obj = Distance(q2R_i(q, self.body_id_j), self.u_i_list[0], n2=self.u_i_list[1], normal=self.n_i)
self._distance_obj = DistanceLineNode(q2R_i(q, self.body_id_j), self.u_P_list_GCS[0], self._n)
# pprint(vars(self._distance_obj))
# distance value
_distance = self._distance_obj._distance - self._distance0
# print "_distance(_evaluate_contact_distance) =", _distance
# time.sleep(10)
return _distance
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 _contact_geometry_GCS(self, q):
"""
Function evaluates contact geometry data in GCS
:param q:
:return:
"""
self.r_i_list_GCS, self.r_jP_GCS = self._contact_geometry_P_GCS(q)
# plane normal in GCS
self._n_GCS = Ai_ui_P_vector(self.n_i_LCS, q2theta_i(q, self.body_id_i))
# distance object
self._distance_obj = DistanceLineNode(q2R_i(q, self.body_id_j), self.r_P_GCS_list[0], normal=self._n_GCS)
# get distance and delta value
_distance = self._distance_obj._distance
_delta = _distance - self.R0_j
return _distance, _delta
def update_contact_points_GCS(self, q):
"""
Function updates coordinates of contact points (geometry) in GCS based on LCS coordinates and vector q
:param q: vector of MBD system coordinates
:return:
"""
# print "update_contact_points_GCS@", __file__
for i, (body_id, uP) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
R = q2R_i(q, body_id)
theta = q2theta_i(q, body_id)
# point coordinate in LCS
rP = cm_lcs2gcs(uP, R, theta)
self.r_P_GCS_list[i] = rP
# distance and delta are equal and multiplied with (-1)
self._distance_vector = self._evaluate_distance_vector(self.r_P_GCS_list[0], self.r_P_GCS_list[1])
# _distance = self._distance_sign = - np.linalg.norm(self._distance_vector, ord=2)
_distance = self._distance_sign = - np.linalg.norm((self.normal * np.dot(self._distance_vector, self.normal)), ord=2)
return _distance, self._distance_sign, self.normal, self.tangent
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 __init__(self,
body_id_i,
body_id_j,
u_iP=np.zeros(2, dtype=float),
node_id_j=None,
properties_dict={},
parent=None):
"""
:param support:
:param body_id_i: id of point mass
: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 point mass-flexible"
super(JointRigidPointMassFlexible,
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 = ["point mass", "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 = [self.e_n_i, 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 = self._parent._parent.bodies[self.body_id_j].e_node(
self.q0, self.node_id_j)[0:2]
self.e_j_grad = self._parent._parent.bodies[self.body_id_j].e_node(
self.q0, self.node_id_j)[2:4]
# vector of joint position in point mass coordinate system
if (u_iP == np.zeros(2)).all():
u_iP = self.e_j - q2R_i(self.q0, self.body_id_i)
self.u_iP_LCS = self.u_P_LCS_list[0] = self.u_iP = u_iP
# list of markers
self.markers, self.r_P_GCS_list = self._create_markers()
# update lists
self._update_lists()
def __init__(self, joint_type, body_id_i, body_id_j, u_iP_CAD=np.array([0, 0]), u_jP_CAD=np.array([0, 0]), u_iQ_CAD=np.array([0, 0]), properties_dict={}, parent=None):
"""
Create a joint object
:param joint_type: type of 2D joint (revolute, prismatic, fixed) as string
:param body_id_i: id of body i
:param body_id_j: id of body j
:param u_iP_CAD: in CAD LCS of a body i
:param u_jP_CAD: in CAD LCS of a body j
:param u_iQ_CAD: in CAD LCS of a body i (only for prismatic joint)
:param parent:
"""
super(Joint, self).__init__(joint_type, parent)
# number
self.joint_id = self.__id.next() # len(joints_list) + 1
self._parent = parent
if self._parent is not None:
self.joints_list = self._parent._parent.joints
else:
self.joints_list = []
# joint type
self.joint_type = joint_type
# dictionary of joint properties
self._dict = properties_dict
# boolean for constant C_q matrix of joint
self.constant = False
# visualization (opengl) properties
self.z_dim = 0.
# get q vector of current MBD system at joint object initialization
self.q0 = self._parent._parent.evaluate_q0()
# spring at joint coordinates
self.spring = None
if self.joint_type not in self._supported_types:
raise ValueError, "Joint type not correct!"
# body ids
self.body_id_i = body_id_i
self.body_id_j = body_id_j
# check ids
if self.joint_type != "slope discontinuity":
self._check_ids(self.body_id_i, self.body_id_j, uiP=u_iP_CAD, ujP=u_jP_CAD)
# body id list
self.body_id_list = [self.body_id_i, self.body_id_j]
self.body_list = []
# body type list
self.body_type_list = ["rigid", "rigid"]
# flexible body data if a flexible body is in joint
self.node_id_i = None
self.node_id_j = None
self.node_id_list = [self.node_id_i, self.node_id_j]
self.element_id_i = None
self.element_id_j = None
self.element_id_list = [self.element_id_i, self.element_id_j]
self.element_ksi_i = None
self.element_ksi_j = None
self.element_ksi_list = [self.element_ksi_i, self.element_ksi_j]
# list of point vectors to joint constraint in CAD lCS of a body
self.u_P_CAD_list = [u_iP_CAD, u_jP_CAD]
# predefined empty list to store point vectors of joint constraint in LCS (center of gravity)
# of each body
self.u_P_LCS_list = []
self.r_P_GCS_list = [None, None]
self.u_iQ_CAD = u_iQ_CAD
# predefined variable of vector C(q, t) constraint equation
self.C0 = None
self.theta0 = None
# time dependent attributes and values
self.C = None
# if self.body_id_i == "ground" or self.body_id_j == "ground":
# _body_id = copy(self.body_id_list)
# _body_id.remove("ground")
# _body_id = _body_id[0]
#
# indx = self.body_id_list.index(_body_id)
# C1 = q2R_i(self.q0, _body_id) + Ai_ui_P_vector(self.u_P_CAD_list[indx], q2theta_i(self.q0, _body_id))
#
# C2 = q2theta_i(self.q0, self.body_id_list[indx])
#
# self.C0 = np.append(C1, C2)
# else:
# self.C0 = np.zeros(3)
# predefined empty list
self.C_q_list = []
self.Q_d_list = []
# list of joint points
# 1 uPi - body i
# 2 uQi - body i
# 3 uPj - body j
self.u_QP_LCS_list = []
# vector of lagrange multipliers at simulation time
self.L = None
# list of vectors of constraint reaction forces for each body in joint
self.Q_c_list = [None, None]
# pair of contact force list
self.contact_bodies_added_to_list = False
self.list_of_contact_force_objects_constructed = False
self.force_list = []
# calculate u_P vector of every body in body LCS
for i, (body_id, _u_P) in enumerate(zip(self.body_id_list, self.u_P_CAD_list)):
if body_id == "ground" or body_id == -1 or body_id is None:
u_P_LCS = _u_P
rP = None
_body = self._parent._parent.ground
else:
# pointer to body object
_body = self._parent._parent.bodies[body_id]
if _body.body_type == "rigid body":
# calculate point vector in body LCS (center of gravity)
u_P_LCS = u_P_cad2cm_lcs(body_id, _body, _u_P, 0)#_body.theta[2]
R_i = q2R_i(self.q0, body_id)
theta_i = q2theta_i(self.q0, body_id)
rP = cm_lcs2gcs(u_P_LCS, R=R_i, theta=theta_i)
else:
u_P_LCS = None
rP = None
self.u_P_LCS_list.append(u_P_LCS)
self.r_P_GCS_list[i] = rP
# list of body pointers
self.body_list.append(_body)
[self.u_iP_LCS, self.u_jP_LCS] = self.u_P_LCS_list
# solution options:
# Discard (default)
# Overwrite (existing file)
# Save to new (next available) file
self._solution_save_options = "discard"
# solution file type, optional: .dat, .xlsx, .csv
self._solution_filetype = ".xlsx"
# markers
self.markers = []
# create forces
self._create_Fn_forces()
# init solution container
self._solution_containers()
# add additional properties from dictionary
self.add_attributes_from_dict(properties_dict)
# visualization properties
self.vtk_actor = None
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
