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 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_theta(self, q):
"""
:return:
"""
theta = np.zeros(3)
if self.body is not None:
if self.body.body_type == "rigid body":
theta[2] = self.body.theta[2] + self.theta0[2]
if self.body.body_type == "flexible body":
e_j_grad = q2e_x_jk(q, self.body.body_id, self.node_id)
theta[2] = np.arctan2(e_j_grad[1], e_j_grad[0])
return theta
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