def evaluate_C0(self, q):
"""
:param q:
:return:
"""
return q2theta_i(q, self.body_id_i) - q2theta_i(q, self.body_id_j)
def evaluate_C(self, q, t=0.):
"""
Function creates C vector for the joint
Args:
bodies - list of all bodies in MBD system
q - vector of absolute coordinates (positions and velocities)
Returns:
C_q matrix for each body in joint
Raises:
"""
self.C = np.zeros(self.n_CNC)
if self.body_id_i != "ground" or self.body_id_j != "ground":
self.C[0:2] = self.evaluate_rijP(q)
elif self.body_id_i == "ground":
self.C[0:2] = Ai_ui_P_vector(self.u_jP,
q2theta_i(q, self.body_id_j))
elif self.body_id_j == "ground":
self.C[0:2] = Ai_ui_P_vector(self.u_iP,
q2theta_i(q, self.body_id_i))
else:
raise ValueError, "Not right!"
self.C = self.C - self.C0
return self.C
def _evaluate_C_q_i(self, q):
"""
Jacobian matrix of constraint equation of rigid body i
:param q:
:return:
"""
C_q_i = np.zeros([self.C_q_dim[0], self.C_q_dim[1][0]])
# e_j_grad = q2e_x_jk(q, self.body_id_j, self.node_id_j)
# theta_i = -np.arctan2(e_j_grad[1], e_j_grad[0])
theta_i = q2theta_i(q, self.body_id_i) # OLD version
C_q_i[0:2, 2] = Ai_theta_ui_P_vector(self.u_iP_LCS, theta_i, 0)
C_q_i[0:2, 0:2] = np.eye(2)
# C_q_i[2:4, 2] = Ai_theta_ui_P_vector(self.u_iR * np.linalg.norm(self.e_j_grad), q2theta_i(q, self.body_id_i), 0)
# C_q_i[2, 2] = np.dot(self.e_j_grad, C_q_i[0:2, 2])
# update of code
ex_j = q2e_x_jk(q, body_id=self.body_id_j, node_id=self.node_id_j)
# print "self.u_iR =", self.u_iR
Ai_theta_uiR = Ai_theta_ui_P_vector(self.u_iR, q2theta_i(q, self.body_id_i), 0)
C_q_i[2, 2] = np.dot(Ai_theta_uiR, ex_j)
return C_q_i
def evaluate_Q_d(self, q):
"""
Function is defined in subclass
:param q:
:return:
"""
Q_d = np.zeros(self.n_CNC)
# get uPi in LCS
_u_P_i = self.u_P_LCS_list[self.body_id_list.index(self.body_id_i)]
# rigid body i data
theta_i = q2theta_i(q, self.body_id_i)
omega_i = q2dtheta_i(q, self.body_id_i)
# evaluate uPi in GCS
u_P_i = Ai_ui_P_vector(_u_P_i, theta_i)
# vector Qd
Q_d[0:2] = u_P_i * (omega_i ** 2)
# Q_d[2] = (omega_i ** 2) * np.dot(u_P_i, self.e_j_grad)
e_x = q2e_x_jk(q, self.body_id_j, self.node_id_j)
Ai_uiR = Ai_ui_P_vector(self.u_iR, theta_i)
de_x = q2de_x_jk(q, self.body_id_j, self.node_id_j)
Ai_theta_uiR = Ai_theta_ui_P_vector(self.u_iP_LCS, q2theta_i(q, self.body_id_i), 0)
# Q_d[2] = np.dot(u_i, e_x) * omega_i + np.dot(u_P_i, de_x)
# Q_d[2] = (np.dot(Ai_uiR, e_x) * (omega_i ** 2)) - (2. * omega_i * np.dot(Ai_theta_ui_P_vector(self.u_iR, theta_i, 0), de_x))
# Q_d[2] = (np.sum(Ai_uiR) * (omega_i ** 2)) - (2. * omega_i * np.dot(Ai_theta_ui_P_vector(self.u_iR, theta_i, 0), de_x))
Q_d[2] = (omega_i ** 2) * (np.dot(Ai_uiR, e_x) - np.dot(Ai_theta_uiR, de_x)) - (omega_i * np.dot(Ai_theta_uiR, e_x))
return Q_d
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_C(self, q, t=None):
"""
Function creates C vector for the joint
Args:
bodies - list of all bodies in MBD system
q - vector of absolute coordinates (positions and velocities)
Returns:
C_q matrix for each body in joint
Raises:
"""
if self.body_id_i != "ground" or self.body_id_j != "ground":
C = self.evaluate_rijP(q)
return C
elif self.body_id_i == "ground":
C = Ai_ui_P_vector(self.u_jP, q2theta_i(q, self.body_id_j))
if self.C0 is not None:
C = C - self.C0
return C
elif self.body_id_j == "ground":
C = Ai_ui_P_vector(self.u_jP, q2theta_i(q, self.body_id_j))
if self.C0 is not None:
C = C - self.C0
return C
else:
raise ValueError, "Not right!"
def evaluate_C0(self, q):
"""
:param q:
:return:
"""
return q2theta_i(q, self.body_id_i) - q2theta_i(q, self.body_id_j)
def evaluate_C_q(self, q):
"""
Function evaluates C_q matrix of prismatic joint
:return:
"""
# calculate additional parameters only once
# assigns properties of body object to variable (object) from list of bodies and body index
# calculates vector of point of joint on each body from cad lcs to cm lcs of a body
# if not self.additional_params_calulated:
# self.u_iP_cm = u_P_cad2cm_lcs(_body_id=self.body_id_i, _u_P=self.u_iP_CAD)
# self.u_jP_cm = u_P_cad2cm_lcs(_body_id=self.body_id_j, _u_P=self.u_jP_CAD)
#
# self.u_iQ_cm = u_P_cad2cm_lcs(_body_id=self.body_id_i, _u_P=self.u_iQ)
#
# self.h_i_cm = self.u_iP_cm - self.u_iQ_cm
#
# self.u_P_list = [self.u_iP_cm, self.u_jP_cm]
#
# self.additional_params_calulated = True
# evaluate h_i
self.h_i = self.evaluate_hi(q)
hi_x, hi_y = self.h_i
self.rijP = self.evaluate_rijP(q)
# predefine empty list to store joint C_q matrix for each body in joint
self.C_q_list = []
# construct C_q matrix for prismatic joint
for body_id in self.body_id_list:
if body_id == "ground":
C_q_body = Joint_C_q_matrix(self.joint_type)
else:
C_q_body = Joint_C_q_matrix(self.joint_type)
C_q_body.matrix[0, 0] = hi_x
C_q_body.matrix[0, 1] = hi_y
C_q_body.matrix[1, 2] = 1
# body i matrix
if body_id == self.body_id_i:
self.rijP = self.evaluate_rijP(q)
C_q_body.matrix[0, 2] = np.dot(self.rijP, np.dot(A_theta_matrix(q2theta_i(q, body_id)), self.h_i_LCS)) + np.dot(self.h_i, np.dot(A_theta_matrix(q2theta_i(q, body_id)), self.u_iP_LCS))
#r_ij_P_Ai_theta_hi_constant(self.rijP, q2theta_i(q, self.body_id_i), self.h_i_LCS)
# body j matrix
if body_id == self.body_id_j:
C_q_body.matrix = -C_q_body.matrix
C_q_body.matrix[0, 2] = - np.dot(self.h_i, np.dot(A_theta_matrix(q2theta_i(q, body_id)), self.u_jP_LCS))
#hi_Ai_theta_ui_P_constant(self.h_i, q2theta_i(q, self.body_id_list[0]), self.u_P_LCS_list[self.body_id_list.index(body_id)])
# append joint C_q matrix to list
self.C_q_list.append(C_q_body)
[C_qi, C_qj] = self.C_q_list
return C_qi.matrix, C_qj.matrix
def evaluate_C_q(self, q):
"""
Function evaluates C_q matrix of prismatic joint
:return:
"""
# calculate additional parameters only once
# assigns properties of body object to variable (object) from list of bodies and body index
# calculates vector of point of joint on each body from cad lcs to cm lcs of a body
# if not self.additional_params_calulated:
# self.u_iP_cm = u_P_cad2cm_lcs(_body_id=self.body_id_i, _u_P=self.u_iP_CAD)
# self.u_jP_cm = u_P_cad2cm_lcs(_body_id=self.body_id_j, _u_P=self.u_jP_CAD)
#
# self.u_iQ_cm = u_P_cad2cm_lcs(_body_id=self.body_id_i, _u_P=self.u_iQ)
#
# self.h_i_cm = self.u_iP_cm - self.u_iQ_cm
#
# self.u_P_list = [self.u_iP_cm, self.u_jP_cm]
#
# self.additional_params_calulated = True
# evaluate h_i
self.h_i = self.evaluate_hi(q)
hi_x, hi_y = self.h_i
self.rijP = self.evaluate_rijP(q)
# predefine empty list to store joint C_q matrix for each body in joint
self.C_q_list = []
# construct C_q matrix for prismatic joint
for body_id in self.body_id_list:
if body_id == "ground":
C_q_body = Joint_C_q_matrix(joint_type_=self.joint_type)
else:
C_q_body = Joint_C_q_matrix(joint_type_=self.joint_type)
C_q_body.matrix[0, 0] = hi_x
C_q_body.matrix[0, 1] = hi_y
C_q_body.matrix[1, 2] = 1
# body i matrix
if body_id == self.body_id_i:
self.rijP = self.evaluate_rijP(q)
C_q_body.matrix[0, 2] = np.dot(self.rijP, np.dot(A_theta_matrix(q2theta_i(q, body_id)), self.h_i_LCS)) + np.dot(self.h_i, np.dot(A_theta_matrix(q2theta_i(q, body_id)), self.u_iP_LCS))
#r_ij_P_Ai_theta_hi_constant(self.rijP, q2theta_i(q, self.body_id_i), self.h_i_LCS)
# body j matrix
if body_id == self.body_id_j:
C_q_body.matrix = -C_q_body.matrix
C_q_body.matrix[0, 2] = - np.dot(self.h_i, np.dot(A_theta_matrix(q2theta_i(q, body_id)), self.u_jP_LCS))
#hi_Ai_theta_ui_P_constant(self.h_i, q2theta_i(q, self.body_id_list[0]), self.u_P_LCS_list[self.body_id_list.index(body_id)])
# append joint C_q matrix to list
self.C_q_list.append(C_q_body)
[C_qi, C_qj] = self.C_q_list
return C_qi.matrix, C_qj.matrix
def evaluate_C0(self, q):
"""
Function evaluates C vector for the joint
:param q: vector of absolute coordinates (positions and velocities)
:
"""
# position (x, y)
C = np.zeros(3)
C[0:2] = self.evaluate_rijP(q)
# rotation (theta)
C[2] = q2theta_i(q, self.body_id_i) - q2theta_i(q, self.body_id_j)
return C
def evaluate_C0(self, q):
"""
Function evaluates C vector for the joint
:param q: vector of absolute coordinates (positions and velocities)
:
"""
# position (x, y)
C = np.zeros(3)
C[0:2] = self.evaluate_rijP(q)
# rotation (theta)
C[2] = q2theta_i(q, self.body_id_i) - q2theta_i(q, self.body_id_j)
return C
def evaluate_Q_d(self, q):
"""
Function evaluates Q_d vector of a joint
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
Args:
bodies - list of all bodies
q - array of ppositions and velosities
N - number of bodies
Returns:
Q_d - vector Q_d for joint
"""
# if body i and body j are not ground
if (self.body_id_i != "ground") and (self.body_id_j != "ground"):
self.Q_d_list = []
for body_id in self.body_id_list:
# vector Q_d object for each body
Q_d_body = Joint_Q_d_vector(self.joint_type)
# evaluated for each body
Q_d_body.Q_d = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, body_id)) * (q2dtheta_i(q, body_id)**2)
# add calculated joint matrix of a body to list
self.Q_d_list.append(Q_d_body)
Q_d = self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d
# construct Q_d vector if body i is ground
elif self.body_id_i == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type,
body_connected_to_ground=True)
Q_d_body.Q_d = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, self.body_id_j)) * (q2dtheta_i(q, self.body_id_j)**2)
Q_d = Q_d_body.Q_d
# construct Q_d vector if body j is ground
elif self.body_id_j == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type,
body_connected_to_ground=True)
Q_d_body.Q_d = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, self.body_id_i)) * (q2dtheta_i(q, self.body_id_i)**2)
Q_d = Q_d_body.Q_d
else:
raise ValueError, "Q_d vector for revolute joint not constructed. Check calculation process."
return Q_d
def evaluate_Q_d(self, q):
"""
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
expanded with z component of moment
"""
# if body i and j are not ground
if self.body_id_i != "ground" and self.body_id_j != "ground":
self.Q_d_list = []
for body_id in self.body_id_list:
Q_d_body = Joint_Q_d_vector(self.joint_type)
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, body_id)) * (q2dtheta_i(q, body_id)**2)
# add calculated joint matrix of a body to list
self.Q_d_list.append(Q_d_body)
Q_d = +(self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d)
elif self.body_id_i == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type,
body_connected_to_ground=True)
Q_d_body.id = 1
body_id_ = self.body_id_list[Q_d_body.id]
# calculate Q_d vector of only non-ground body
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(
u_P=self.u_P_list[Q_d_body.id], theta=q2theta_i(
q, body_id_)) * (q2dtheta_i(q, body_id_)**2)
Q_d = Q_d_body.Q_d
elif self.body_id_j == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type,
body_connected_to_ground=True)
body_id_ = self.body_id_list[Q_d_body.id]
# calculate Q_d vector of only non-ground body
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, body_id_)) * (q2dtheta_i(q, body_id_)**2)
Q_d = Q_d_body.Q_d
else:
raise ValueError, "Q_d vector for fixed joint not constructed. Check calculation process."
return Q_d
def _contact_velocity(self, q):
"""
Function evaluates relative contact velocity at contact point in revolute clearance joint.
:param q: array of coordinates (r, theta) and velocities (dR, dheta=omega) of MBD system
"""
dr_P = []
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)
# add to list
dr_P.append(dr_P_body)
_dq = dr_P[1] - dr_P[0]
# relative contact velocity
# normal direction
_dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
_dq_t = np.dot(_dq, self._t_GCS)
return _dq_n, _dq_t
def _contact_velocity(self, q):
"""
Args:
q:
Returns:
"""
dr_P = []
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
# body velocity, R, theta
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)
# print 'dr_P_body=', dr_P_body, 'body_id=', body_id
# add to list
dr_P.append(dr_P_body)
_dq = dr_P[1] - dr_P[0]
self._dq_n = np.dot(_dq, self.normal)
# print 'dq_n=', self._dq_n, 'normal=', self.normal, _dq
self._dq_t = np.dot(_dq, self.tangent)
return self._dq_n, self._dq_t
def _contact_velocity(self, q):
"""
Function evaluates relative contact velocity at contact point in revolute clearance joint.
:param q: array of coordinates (r, theta) and velocities (dR, dhete=omega) of MBD system
"""
dr_P = []
print "self.u_P_LCS_list =", self.u_P_LCS_list, type(self.u_P_LCS_list)
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)
# add to list
dr_P.append(dr_P_body)
_dq = dr_P[0] - dr_P[1]
# relative contact velocity
# normal direction
_dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
_dq_t = np.dot(_dq, self._t_GCS)
return _dq_n, _dq_t
def contact_velocity(self, q):
"""
Function evaluates relative contact velocity vectors in normal and tangent direction
:param q:
:return:
"""
dr_P = [np.zeros(2, dtype="float32"), np.zeros(2, dtype="float32")]
for i, (body_id,
u_P) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
# body velocity, R, theta
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_i = dr_contact_point_uP(dR, theta, dtheta, u_P)
# update list
dr_P[i] = dr_P_i
# relative contact velocity vector
_dq = dr_P[1] - dr_P[0]
# relative contact velocity
# normal direction
self._n_GCS = self.normal
self._dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
self._t_GCS = self.tangent
self._dq_t = np.dot(_dq, self._t_GCS)
return self._dq_n, self._dq_t
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 _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 contact_velocity(self, q):
"""
positive value - bodies are approaching
negative values - bodies are moving apart
:return:
"""
dr_P = [np.zeros(2, dtype="float32"), np.zeros(2, dtype="float32")]
for i, (body_id,
u_P) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
# body velocity, R, theta
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_i = dr_contact_point_uP(dR, theta, dtheta, u_P)
# update list
dr_P[i] = dr_P_i
# relative contact velocity vector
_dq = dr_P[1] - dr_P[0]
# relative contact velocity
# normal direction
self._n_GCS = self.n_list[self.index]
self._dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
self._t_GCS = self.t_list[self.index]
self._dq_t = np.dot(_dq, self._t_GCS)
return self._dq_n, self._dq_t
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 _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 _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 _contact_velocity(self, q):
"""
Function evaluates relative contact velocity vectors in normal and tangent direction
"""
dr_P = []
for body_id, _R, _n, _t in zip(self.body_id_list, self.R0_list, self._n_list, self._t_list):
# contact point in body LCS
_uP = _R * _n
# body velocity, R, theta
_dR = q2dR_i(q, body_id)
_theta = q2theta_i(q, body_id)
# dtheta - omega
_dtheta = q2dtheta_i(q, body_id)
# velocity at contact point (in GCS) on a body
_dr_P = dr_contact_point_uP(_dR, _theta, _dtheta, _uP)
# appned to list
dr_P.append(_dr_P)
# relative contact velocity vector
_dq = dr_P[1] - dr_P[0]
# relative contact velocity (scalar value)
# normal direction
_dq_n = np.dot(_dq, self._n)
# tangent direction
_dq_t = np.dot(_dq, self._t)
return _dq_n, _dq_t
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_velocity(self, q):
"""
Function evaluates relative contact velocity vectors in normal and tangent direction
"""
dr_P = []
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
# body velocity, R, theta
_dR = q2dR_i(q, body_id)
_theta = q2theta_i(q, body_id)
# dtheta - omega
_dtheta = q2dtheta_i(q, body_id)
# velocity at contact point (in GCS) on a body
_dr_P = dr_contact_point_uP(_dR, _theta, _dtheta, u_P)
# appned to list
dr_P.append(_dr_P)
# relative contact velocity vector
_dq = dr_P[1] - dr_P[0]
# relative contact velocity (scalar value)
# normal direction
_dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
_dq_t = np.dot(_dq, self._t_GCS)
return _dq_n, _dq_t
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 evaluate_hi(self, q):
"""
:param q:
:return:
"""
h_i = np.dot(A_matrix(q2theta_i(q, self.body_id_i)), self.h_i_LCS)
return h_i
def evaluate_hi(self, q):
"""
:param q:
:return:
"""
h_i = np.dot(A_matrix(q2theta_i(q, self.body_id_i)), self.h_i_LCS)
return h_i
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 __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 evaluate_Q_d(self, q):
"""
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
expanded with z component of moment
"""
Q_d = np.zeros(self.n_CNC)
# get uPi in LCS
_u_P_j = self.u_P_LCS_list[self.body_id_list.index(self.body_id_j)]
# rigid body i data
theta_j = q2theta_i(q, self.body_id_j)
omega_j = q2dtheta_i(q, self.body_id_j)
# evaluate uPi in GCS
u_P_i = -Ai_ui_P_vector(_u_P_j, theta_j)
# vector Qd
Q_d[0:2] = u_P_i * (omega_j**2)
# # if body i and j are not ground
# if self.body_id_i != "ground" and self.body_id_j != "ground":
# self.Q_d_list = []
# for body_id in self.body_id_list:
# Q_d_body = Joint_Q_d_vector(self.joint_type)
#
# Q_d_body.Q_d[0:2] = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, body_id)) * (q2dtheta_i(q, body_id) ** 2)
#
# # add calculated joint matrix of a body to list
# self.Q_d_list.append(Q_d_body)
#
# Q_d = +(self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d)
#
# elif self.body_id_i == "ground":
# Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
#
# Q_d_body.id = 1
#
# body_id_ = self.body_id_list[Q_d_body.id]
# # calculate Q_d vector of only non-ground body
# Q_d_body.Q_d[0:2] = Ai_ui_P_vector(u_P=self.u_P_list[Q_d_body.id], theta=q2theta_i(q, body_id_)) * (q2dtheta_i(q, body_id_) ** 2)
#
# Q_d = Q_d_body.Q_d
#
# elif self.body_id_j == "ground":
# Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
# body_id_ = self.body_id_list[Q_d_body.id]
#
# # calculate Q_d vector of only non-ground body
# Q_d_body.Q_d[0:2] = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, body_id_)) * (q2dtheta_i(q, body_id_) ** 2)
#
# Q_d = Q_d_body.Q_d
# else:
# raise ValueError, "Q_d vector for fixed joint not constructed. Check calculation process."
return Q_d
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 _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 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_Q_d(self, q):
"""
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
expanded with z component of moment
"""
# if body i and j are not ground
if self.body_id_i != "ground" and self.body_id_j != "ground":
self.Q_d_list = []
for body_id in self.body_id_list:
Q_d_body = Joint_Q_d_vector(self.joint_type)
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, body_id)) * (q2dtheta_i(q, body_id) ** 2)
# add calculated joint matrix of a body to list
self.Q_d_list.append(Q_d_body)
Q_d = +(self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d)
elif self.body_id_i == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
Q_d_body.id = 1
body_id_ = self.body_id_list[Q_d_body.id]
# calculate Q_d vector of only non-ground body
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(u_P=self.u_P_list[Q_d_body.id], theta=q2theta_i(q, body_id_)) * (q2dtheta_i(q, body_id_) ** 2)
Q_d = Q_d_body.Q_d
elif self.body_id_j == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
body_id_ = self.body_id_list[Q_d_body.id]
# calculate Q_d vector of only non-ground body
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, body_id_)) * (q2dtheta_i(q, body_id_) ** 2)
Q_d = Q_d_body.Q_d
else:
raise ValueError, "Q_d vector for fixed joint not constructed. Check calculation process."
return Q_d
def evaluate_C(self, q, t=None):
"""
Function creates C vector for the joint
Args:
bodies - list of all bodies in MBD system
q - vector of absolute coordinates (positions and velocities)
Returns:
C_q matrix for each body in joint
Raises:
"""
self.C = np.zeros(2)
# dot product of orthogonal vectors hi and rijP is 0
rijP = self.evaluate_rijP(q)
hi = self.evaluate_hi(q)
self.C[0] = np.dot(hi, rijP)
# angle between connected bodies is constant
self.C[1] = q2theta_i(q, self.body_id_i) - q2theta_i(q, self.body_id_j) - self.theta0
return self.C
def evaluate_C(self, q, t=None):
"""
Function creates C vector for the joint
Args:
bodies - list of all bodies in MBD system
q - vector of absolute coordinates (positions and velocities)
Returns:
C_q matrix for each body in joint
Raises:
"""
C = np.zeros(2)
# dot product of orthogonal vectors hi and rijP is 0
rijP = self.evaluate_rijP(q)
hi = self.evaluate_hi(q)
C[0] = np.dot(hi, rijP)
# angle between connected bodies is constant
C[1] = q2theta_i(q, self.body_id_i) - q2theta_i(q, self.body_id_j) - self.theta0
return C
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_dl(self, q, I_r_ij_P):
"""
:return:
"""
# vector of dq_i, dq_j
dq = np.array([np.append(q2dR_i(q, self.body_id_i), q2dtheta_i(q, self.body_id_i)),
np.append(q2dR_i(q, self.body_id_j), q2dtheta_i(q, self.body_id_j))]).flatten()
# dr_ij_P_dq
dr_ij_P_dq_ = dr_ij_P_dq(body_id_i=self.body_id_i,
theta_i=q2theta_i(q, self.body_id_i),
u_iP=self.u_iP_LCS,
body_id_j=self.body_id_j,
theta_j=q2theta_i(q, self.body_id_j),
u_jP=self.u_jP_LCS)
# velocity of deformation of spring length
dl = np.dot(I_r_ij_P, np.dot(dr_ij_P_dq_, dq))
return dl
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()
# self.C[2] = np.dot(rR_i, self.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 _evaluate_C_q_j(self, q):
"""
:param q:
:return:
"""
C_q_j = np.zeros([self.n_CNC, self.C_q_dim[1][1]])
C_q_j[0:2, 0:2] = -np.eye(2)
C_q_j[:, -1] = Ai_theta_ui_P_vector(self.u_P_LCS_list[1],
q2theta_i(q, self.body_id_j), 1)
return C_q_j
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 evaluate_Q_d(self, q):
"""
Function evaluates Q_d vector of a joint
Q_d equation according to eq. 3.133 (example 3.14) in Computational Dynamics 3rd Ed. (A.A.Shabana)
Args:
bodies - list of all bodies
q - array of ppositions and velosities
N - number of bodies
Returns:
Q_d - vector Q_d for joint
"""
Q_d = np.zeros(2)
# get theta i, j
theta_i = q2theta_i(q, self.body_id_i)
theta_j = q2theta_i(q, self.body_id_j)
# get translational absolute velocity of body - R i, j
dR_i = q2dR_i(q, self.body_id_i)
dR_j = q2dR_i(q, self.body_id_j)
# get rotational absolute velocity of body i, j
dtheta_i = q2dtheta_i(q, self.body_id_i)
dtheta_j = q2dtheta_i(q, self.body_id_j)
# evaluate h_i
hi = self.evaluate_hi(q)
# evaluate distance between
rijP = self.evaluate_rijP(q)
Q_d_21 = 2 * theta_i * np.dot(np.dot(A_theta_matrix(theta_i), hi), (dR_i - dR_j))
Q_d_22 = (dtheta_i ** 2) * (np.dot(self.u_iP_LCS, self.u_iQ_LCS) - np.dot(rijP, hi))
Q_d_23 = 2 * dtheta_i * dtheta_j * np.dot(np.dot(A_theta_matrix(theta_i), hi), np.dot(A_theta_matrix(theta_j), self.u_jP_LCS))
Q_d_24 = (dtheta_j ** 2) * np.dot(hi, self.u_jP_LCS)
Q_d[0] = -Q_d_21 - Q_d_22 + Q_d_23 - Q_d_24
return Q_d
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_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 evaluate_C_q(self, q):
"""
Function evaluates C_q matrix
:return:
"""
# predefine empty list to store joint C_q matrix for each body in joint
self.C_q_list = []
# construct C_q matrix for each body in joint
for body_id, _u_P, _id in zip(self.body_id_list, self.u_P_LCS_list, [0, 1]):
if body_id == "ground":
C_q_body = Joint_C_q_matrix(self.joint_type)
else:
C_q_body = Joint_C_q_matrix(self.joint_type)
# create body Cq matrix
C_q_body.matrix[:, -1] = Ai_theta_ui_P_vector(_u_P, q2theta_i(q, body_id), _id)
# append joint C_q matrix to list
self.C_q_list.append(C_q_body)
[C_qi, C_qj] = self.C_q_list
return C_qi.matrix, C_qj.matrix
def evaluate_Q_d(self, q):
"""
Function evaluates Q_d vector of a joint
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
Args:
bodies - list of all bodies
q - array of ppositions and velosities
N - number of bodies
Returns:
Q_d - vector Q_d for joint
"""
# if body i and body j are not ground
if (self.body_id_i != "ground") and (self.body_id_j != "ground"):
self.Q_d_list = []
for body_id in self.body_id_list:
# vector Q_d object for each body
Q_d_body = Joint_Q_d_vector(self.joint_type)
# evaluated for each body
Q_d_body.Q_d = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, body_id)) * (q2dtheta_i(q, body_id) ** 2)
# add calculated joint matrix of a body to list
self.Q_d_list.append(Q_d_body)
Q_d = self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d
# construct Q_d vector if body i is ground
elif self.body_id_i == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
# Q_d_body.id = 1
#
# _body_id = self.body_id_list[Q_d_body.id]
#
# # calculate Q_d vector of only non-ground body
# Ai_ui_P_vector_ = Ai_ui_P_vector(u_P=self.u_P_list[Q_d_body.id], _theta=q2theta_i(q_, _body_id))
#
#
# dtheta_body = q2dtheta_i(q_, _body_id)
#
# Q_d_body.Q_d = Ai_ui_P_vector_ * (dtheta_body ** 2)
Q_d_body.Q_d = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, self.body_id_j)) * (q2dtheta_i(q, self.body_id_j) ** 2)
Q_d = Q_d_body.Q_d
# construct Q_d vector if body j is ground
elif self.body_id_j == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
# _body_id = self.body_id_list[Q_d_body.id]
#
# # get evaluated vector to point of kinematic constraint u_P
# _u_P = self.u_P_list[Q_d_body.id]
# # theta of body from vector q
# _theta = q2theta_i(q, _body_id)
# # calculate Q_d vector of only non-ground body
# Ai_ui_P_vector_ = Ai_ui_P_vector(_u_P, _theta)
# # omega of body from vector q
# dtheta_body = q2dtheta_i(q, _body_id)
#
# Q_d_body.Q_d = Ai_ui_P_vector_ * dtheta_body ** 2
Q_d_body.Q_d = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, self.body_id_i)) * (q2dtheta_i(q, self.body_id_i) ** 2)
Q_d = Q_d_body.Q_d
else:
raise ValueError, "Q_d vector for revolute joint not constructed. Check calculation process."
return Q_d
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
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 evaluate_C_q(self, q):
"""
Function evaluates C_q matrix
:return:
"""
self.C_q_list = []
# construct C_q matrix for fixed joint
for body_id, _u_P in zip(self.body_id_list, self.u_P_LCS_list):
if body_id == "ground":
joint_C_q_matrix_body = Joint_C_q_matrix(joint_type_=self.joint_type)
else:
joint_C_q_matrix_body = Joint_C_q_matrix(joint_type_=self.joint_type)
joint_C_q_matrix_body.matrix[[0, 1], -1] = Ai_theta_ui_P_vector(_u_P, q2theta_i(q, body_id), joint_C_q_matrix_body.id)
self.C_q_list.append(joint_C_q_matrix_body)
[C_qi, C_qj] = self.C_q_list
return C_qi.matrix, C_qj.matrix
