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 _create_markers(self): """ Function creates markers :return: """ markers = [] # create markers for joint centers for i, u_P in enumerate(self.u_CP_LCS_list): # get correct body object body_id = body = None if i == 0 or i == 1: body = self.body_list[0] body_id = self.body_id_list[0] if i == 2: body = self.body_list[1] body_id = self.body_id_list[1] # node coordinates in GCS rP = u_P_lcs2gcs(u_P, self.q0, body_id) # add 3rd dimension for visualization rP = np.array(np.append(rP, self.z_dim), dtype="float32") if body is not None: # create marker object marker = Marker(rP, parent=self) # append marker to list of contact markers markers.append(marker) return markers
def update_F(self, q, step=None, F=np.zeros(2), u_P=np.zeros(2), r_P=np.zeros(2)): """ :param step: :param Fx: :param Fy: :param Mz: :return: """ if step is None: pass else: self._step_num_solution_container.append(step) # update applied force vector self._update_F(F) # update position vector of applied force # LCS if (u_P == np.zeros(2)).all(): u_P = self.u_iP_f self._update_u_P(u_P) # GCS if (r_P == np.zeros(2)).all() and hasattr(q, "__len__"): r_P = u_P_lcs2gcs(u_P, q, self.body_id) self._update_r_P(r_P)
def reset(self, q): """ Function defined in subclass :return: """ for i, (body_id, uP, node_id) in enumerate( zip(self.body_id_list, self.u_P_LCS_list, self.node_id_list)): self.r_P_list[i] = u_P_lcs2gcs(uP, q, body_id, node_id=node_id)
def _pin_GCS(self, q): """ Function updates and calculates position of a pin center in global coordinates - GCS :param q: :return: """ CP_GCS = u_P_lcs2gcs(self.u_jP, q, self.body_id_j) return CP_GCS
def _contact_geometry_P_GCS(self, q): """ Function evaluates position of body points (sphere center) and points that define edge - line :return: """ r_i_list_GCS = [] for i, body_id in enumerate(self.body_id_list): # geometry points of line if i == 0: for u_iP in self.u_i_list_LCS: if u_iP is not None: r_iP = u_P_lcs2gcs(u_iP, q, body_id) r_i_list_GCS.append(r_iP) # circle center if i == 1: r_jP_GCS = u_P_lcs2gcs(self.u_jP_LCS, q, body_id) return r_i_list_GCS, r_jP_GCS
def _pin_GCS(self, q): """ Function updates and calculates position of a pin center in global coordinates - GCS :param q: :return: """ # CP_GCS = q2R_i(q, self.body_id_i) + Ai_ui_P_vector(self.u_iP, q2theta_i(q, self.body_id_i)) CP_GCS = u_P_lcs2gcs(self.u_iP, q, self.body_id_i) return CP_GCS
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() # C[2] = np.dot(rR_i, 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 _slot_GCS(self, q): """ :param q: :return: """ slot_CP_GCS = [] for CP in self.u_CP_slot_LCS: _uP_gcs = u_P_lcs2gcs(CP, q, self.body_id_i) slot_CP_GCS.append(_uP_gcs) return slot_CP_GCS
def evaluate_rijP(self, q): """ Length of spring at time when vector of absolute coordinates is equal to q """ for i, (body_id, uP) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)): self.r_P_list[i] = u_P_lcs2gcs(uP, q, body_id) r_ij_P = self.r_P_list[0] - self.r_P_list[1] return r_ij_P
def _slot_GCS(self, q): """ :param q: :return: """ slot_CP_GCS = [] for CP in self.u_CP_slot_LCS: _uP_gcs = u_P_lcs2gcs(CP, q, self.body_id_j) slot_CP_GCS.append(_uP_gcs) return slot_CP_GCS
def _contact_geometry_CP_GCS(self, q): """ Function calculates position of centers (CP - Center Points) of revolute joint pin/hole in GCS :param q: a vector of coordinates of the system (numpy array) :return u_CP_list_GCS: a list of two arrays (vectors) of """ # calculate position of pin/hole centers of each body in GCS for i, (body_id, u_P) in enumerate(zip(self.body_id_list, self.u_CP_LCS_list)): # axis center of revolute joint of each body in GCS self.r_CP_GCS_list[i] = u_P_lcs2gcs(u_P, q, body_id) # convert to 32bit float to display in opengl scene return np.array(self.r_CP_GCS_list, dtype="float32")
def _reset(self, q): """ :return: """ # 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]
def _contact_geometry_CP_GCS(self, q): """ Function calculates position of centers (CP - Center Points) of revolute joint pin/hole in GCS :param q: a vector of coordinates of the system (numpy array) :return u_CP_list_GCS: a list of two arrays (vectors) of """ # calculate position of pin/hole centers of each body in GCS u_CP_GCS_list = [] print "q =", q for body_id, u_P in zip(self.body_id_list, self.u_CP_LCS_list): print "u_P =", u_P # axis center of revolute joint of each body in GCS u_P_GCS = u_P_lcs2gcs(u_P, q, body_id) print "u_P_GCS =", u_P_GCS u_CP_GCS_list.append(u_P_GCS) # reformat to 32bit float to display in opengl scene return np.array(u_CP_GCS_list, dtype="float32")
def _evaluate_Q_e_rigid(self, t, q, F): """ Evaluate generalized external force for rigid body :param t: :param q: :return: """ # position of force in GCS self.r_P_GCS = u_P_lcs2gcs(self.u_P_LCS, q, self.body_id) # get theta of body from q vector theta = q2theta_i(q, self.body_id) # construct a matrix (function of vector q) matrix = Force_Q_e_matrix(self.body_id, self.u_iP_f, theta) # form a vector Q_e = np.dot(matrix.matrix, F) return Q_e
def reset(self, q0): """ :return: """ # set q0 self.q0 = q0 self.Fx = 0. self.Fy = 0. self.Mz = 0. if self._body.body_type == "rigid body": self.r_P_GCS = u_P_lcs2gcs(self.u_P_LCS, self.q0, self.body_id) if self._body.body_type == "flexible body": self.r_P_GCS = self.q0 self.F = self._evaluate_F(0.) self.update_vtk_data()
def _create_markers(self): """ Function create markers :return: """ markers = [] self.r_P_list = [] for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list): if body_id == "ground" or body_id == -1: # pointer to body object body = self._parent._parent.ground r_P = u_P else: r_P = u_P_lcs2gcs(u_P, self.q0, body_id) # pointer to body object if self._parent is not None: body = self._parent._parent.bodies[body_id] else: body = None self.r_P_list.append(r_P) if body is not None: # create marker object r_node = np.array(np.append(r_P, self.z_dim), dtype="float32") u_node = np.array(np.append(u_P, self.z_dim), dtype="float32") marker = Marker(r_node, uP=u_node, body_id=body, parent=body) # append marker object # to body list of markers body.markers.append(marker) # to markers list markers.append(marker) self.body_list.append(body) return markers
def evaluate_C(self, q, t=None): """ Function evaluates C for the joint :param q: :param t: :return: """ # 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 = rP_i - rP_j return self.C