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