def __init__(self, r_iP, r_jP, parent=None): """ Constructor of a distance object :param u_iP: center of body i hole in GCS coordinates _param u_jP: center of body j pin in GCS coordinates :return: """ # parent self._parent = parent # body ids # i - free point of pin body id # j - line body of slot body id self.body_id_i = self._parent.body_id_i self.body_id_j = self._parent.body_id_j self.body_id_list = [self.body_id_i, self.body_id_j] # points in GCS self.r_jP = r_jP self.r_iP = r_iP self._distance_vector = self.r_jP - self.r_iP self._distance = np.linalg.norm(self._distance_vector, ord=2) # normal in GCS of cylindrical surface self.normal = self._distance_vector / self._distance # tangent in GCS self.tangent = n2t(self.normal)
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 _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 _contact_geometry_GCS(self, q): """ Function calculates distance between center points os spheres and indentation based on radius of sphere i and j Evaluated parameters: :param _distance: distance between centers of sphere i and j :param _delta: penetration depth at contact point """ # create distance object # print "q =", q # print "i =", self.body_id_i # print "j =", self.body_id_j # print "i =", q2R_i(q, self.body_id_i) # print "j =", q2R_i(q, self.body_id_j) self._distance_obj = Distance(q2R_i(q, self.body_id_i), q2R_i(q, self.body_id_j)) # distance _d = self._distance_obj._distance # delta _delta = self._distance_obj._distance - self._distance0 # normal in GCS _n_GCS = -self._distance_obj._distance_vector / self._distance_obj._distance # tangent in GCS _t_GCS = n2t(_n_GCS) return _d, _delta, _n_GCS, _t_GCS
def _get_contact_geometry_data(self, q): """ Function locates and creates contact point vector in GCS based on position of both bodies in contact :param q: :return: """ # print "_get_contact_geometry_data()" if self.pin_in_section_jPjR: # contact point on body j - slot # GCS self.u_P_GCS_list[1] = self.u_jP_GCS = self._distance_obj.contact_point_on_line_GCS() # LCS self.u_jP_LCS = u_P_gcs2lcs(self.u_jP_GCS, q, self.body_id_j) # print "self.u_jP_LCS =", self.u_jP_LCS # get normal in GCS self._n_GCS = self._distance_obj.normal # print "self._n_GCS =", self._n_GCS elif self.pin_in_section_jP or self.pin_in_section_jR: # get normal in GCS self._n_GCS = self._distance_obj._distance_vector / self._distance_obj._distance else: raise ValueError, "Contact in slot section not detected!" # get tangent in CS self._t_GCS = n2t(self._n_GCS) self._contact_point_found = True
def _contact_geometry_GCS(self, q): """ Function calculates distance between center points (eccentricity vector) and indentation based on radius of pin/hole Function calculates actual contact point on each body in body LCS and GCS :param q: vector of states of MBD system :return _distance: distance between center of pin and hole :paramm _delta: depth of deformation (indentation, penetration) :return _n_GCS: normal of contact in GCS _return _t_GCS: tangent of contact in GCS """ # a list of center point of pin and hole on each body in GCS self.r_CP_GCS_list = self._contact_geometry_CP_GCS(q) # calculate distance between axis of both bodies in revolute joint self._distance_obj = DistanceRCJ(self.r_CP_GCS_list[0], self.r_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 # unit vector in direction from one center to another (pin to hole) n_GCS = self._distance_obj._distance_vector / self._distance_obj._distance # normal unit vector for body i, j n_GCS_list = [n_GCS, -n_GCS] # tangent in GCS t_GCS = n2t(n_GCS) # calculate a actual contact point in revolute clearance joint of each body in GCS self.r_P_GCS_list = copy.copy(self.r_P_GCS_0_list) # evaluate actual contact point in LCS of each body and in GCS for i, (_u_CP_GCS, _u_CP_LCS, _R0, n_i) in enumerate(zip(self.r_CP_GCS_list, self.u_CP_LCS_list, self.R0_list, n_GCS_list)): # contact point in GCS _u_P_GCS = _u_CP_GCS + _R0 * n_i # add to list self.r_P_GCS_list[i] = _u_P_GCS return distance, delta, n_GCS, t_GCS
def __init__(self, r_iP, r_jP, r_jR=None, normal=None, q=None, node_body=None, edge_body=None, parent=None): """ Constructor of a distance object that calculates distance (value, scalar) of free node to edge :param n0: free node of a body in body LCS :param r_jP: start node of an edge of a body in body LCS :param r_jR: end node of an edge of a body in body LCS :param normal: normal of an edge in body LCS :param q: vector of generalized coordinates at time t :param node_body: pointer to node body object :param edge_body: """ # parent self._parent = parent # default value for attribute self._distance = np.inf # initialize data containers self._data_container() # node on body i - free node self.r_iP = r_iP # node on body j self.r_jP = r_jP # normal self.normal = normal # tangent if self.normal is not None: self.tangent = n2t(self.normal) else: self.tangent = None if node_body is not None and edge_body is not None: # pointers to edge and node body self.node_body = node_body self.node_body_id = node_body.body_id self.edge_body = edge_body self.edge_body_id = edge_body.body_id # edge vector from 1 to 2 self.r_jRr_jP = r_jR - r_jP # print "self.edge =", self.r_jRr_jP # angle = np.arccos(np.dot(r_jP, r_jR)) # print "angle =", angle # edge nodes self.r_jP = r_jP self.r_jR = r_jR # edge vector self.edge = r_jR - r_jP # evaluate direction of edge vector it has to be in direction of normal self._check_direction(r_jP, r_jR) # unit edge vector self.r_jRr_jP_e = self.edge / np.linalg.norm(self.edge) # max penetration depth attribute assigned from body attribute if edge_body is not None: self.max_penetration_depth = edge_body.max_penetration_depth else: self.max_penetration_depth = 1E-4 self.min_penetration_depth = 10 * self.max_penetration_depth # evaluate distance # evaluate distance: node to node if r_jR is None: self._distance, self._inside = self._vector_length( self.r_iP, self.r_jP) # evaluate distance: node to edge else: self._evaluate_distance_2D(q) # calculate tangent based on normal self.tangent = Ai_ui_P_vector(self.normal, np.pi / 2)