def evaluate_C_q(self, q=None):
"""
Function evaluates C_q matrix for every element connected by this support (joint) object
:param q:
:return:
"""
C_q_list = []
# construct C_q matrix for fixed support (joint)
for body_id, node_id in zip(self.body_id_list, self.node_id_list):
# ground
if type(body_id) is not int:
C_q = Joint_C_q_matrix(self.joint_type, parent=self)
# actual body object
else:
C_q = Joint_C_q_matrix(self.joint_type,
body_id=body_id,
parent=self)
C_q.matrix = self._parent._parent.bodies[
body_id].evaluate_C_q_hinged(node_id)
C_q_list.append(C_q)
[C_qi, C_qj] = C_q_list
return C_qi.matrix, C_qj.matrix
def evaluate_C_q(self, q):
"""
Function evaluates C_q matrix for every element connected by this support (joint) object
:param q:
:return:
"""
self.C_q_list = []
# construct C_q matrix for fixed support (joint)
for body_id, node_id in zip(self.body_id_list, self.node_id_list):
if body_id == "ground":
C_q = Joint_C_q_matrix(self.joint_type, parent=self)
else:
C_q = Joint_C_q_matrix(self.joint_type,
body_id=body_id,
parent=self)
C_q.matrix = self._parent._parent.bodies[
body_id].evaluate_C_q_fixed(node_id)
self.C_q_list.append(C_q)
[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(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_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
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_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(self.joint_type)
else:
joint_C_q_matrix_body = Joint_C_q_matrix(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
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
