def __init__(self, _type, body_id_i, body_id_j, u_iP, u_jP, R0_i, R0_j, properties_dict=[], parent=None): """ Constructor of class contact of revolute clearance joint :param _type: type of clearance joint or contact :param body_id_i: id of hole body :param body_id_j: id of pin body :param u_iP: vector to center of a hole on body i in body LCS :param u_jP: vector to center of a pin on body j in body LCS :param R0_i: radius of a hole :param R0_j: radius of a pin :param properties_dict: additioanl parameters to override default values or add new parameters """ # number # self.contact_id = self.__id.next() # name as string self._name = "RC_Joint_"# + str(self.contact_id) # this has to be after attributes contact_id and _name as name is constructed from contact_id and _name super(RevoluteClearanceJoint, self).__init__(_type, body_id_i, body_id_j, name=self._name, properties_dict=properties_dict, parent=parent) self._parent = parent # joint type self._type = "Revolute Clearance Joint" # vector of axis on revolute joint in LCS of a body i, j self.u_iP = u_iP self.u_jP = u_jP # predefined empty list of center point or clearance joint (to center of pin/hole) in body LCS self.u_CP_LCS_list = [self.u_iP, self.u_jP] # centers of revolute clearance joint in GCS self.u_iCP_GCS = np.zeros(2) self.u_jCP_GCS = np.zeros(2) self.u_CP_GCS_list = [self.u_iCP_GCS, self.u_jCP_GCS] # clearance parameters self.R0_i = R0_i self.R0_j = R0_j self.R0_list = [self.R0_i, self.R0_j] # radial clearance self._radial_clearance = abs(R0_i - R0_j) # contact model if self.contact_model_type is None: self.contact_model_type = "hertz" # print "self.properties_contact_model =", self.properties_contact_model self.contact_model = ContactModelCylinder(self.contact_model_type, properties_dict=self.properties_contact_model, parent=self) # joint body ids self.body_id_i = body_id_i self.body_id_j = body_id_j self.body_id_list = [self.body_id_i, self.body_id_j] # list of markers self.markers = self._create_markers()
class RevoluteClearanceJoint(Contact): """ classdocs """ # __id = itertools.count(0) def __init__(self, _type, body_id_i, body_id_j, u_iP, u_jP, R0_i, R0_j, properties_dict=[], parent=None): """ Constructor of class contact of revolute clearance joint :param _type: type of clearance joint or contact :param body_id_i: id of hole body :param body_id_j: id of pin body :param u_iP: vector to center of a hole on body i in body LCS :param u_jP: vector to center of a pin on body j in body LCS :param R0_i: radius of a hole :param R0_j: radius of a pin :param properties_dict: additioanl parameters to override default values or add new parameters """ # number # self.contact_id = self.__id.next() # name as string self._name = "RC_Joint_"# + str(self.contact_id) # this has to be after attributes contact_id and _name as name is constructed from contact_id and _name super(RevoluteClearanceJoint, self).__init__(_type, body_id_i, body_id_j, name=self._name, properties_dict=properties_dict, parent=parent) self._parent = parent # joint type self._type = "Revolute Clearance Joint" # vector of axis on revolute joint in LCS of a body i, j self.u_iP = u_iP self.u_jP = u_jP # predefined empty list of center point or clearance joint (to center of pin/hole) in body LCS self.u_CP_LCS_list = [self.u_iP, self.u_jP] # centers of revolute clearance joint in GCS self.u_iCP_GCS = np.zeros(2) self.u_jCP_GCS = np.zeros(2) self.u_CP_GCS_list = [self.u_iCP_GCS, self.u_jCP_GCS] # clearance parameters self.R0_i = R0_i self.R0_j = R0_j self.R0_list = [self.R0_i, self.R0_j] # radial clearance self._radial_clearance = abs(R0_i - R0_j) # contact model if self.contact_model_type is None: self.contact_model_type = "hertz" # print "self.properties_contact_model =", self.properties_contact_model self.contact_model = ContactModelCylinder(self.contact_model_type, properties_dict=self.properties_contact_model, parent=self) # joint body ids self.body_id_i = body_id_i self.body_id_j = body_id_j self.body_id_list = [self.body_id_i, self.body_id_j] # list of markers self.markers = self._create_markers() def _create_markers(self): """ Function creates markers :return: """ markers = [] for body, _u_P in zip(self.body_list, self.u_CP_LCS_list): # node coordinates node = np.array(np.append(_u_P, self.z_dim), dtype='float32') # create marker object _marker = Marker(node, visible=True, parent=body) # append marker to list of body markers body.markers.append(_marker) # append marker to list of revolute clearance joint markers markers.append(_marker) return markers def check_for_contact(self, q): """ Function check for contact of revolute clearance joint """ # print "check_for_contact()" # evaluated distance, delta self._distance, self._delta = self._contact_geometry_GCS(q) print "self._delta =", self._delta # add distance value to container # self._distance_solution_container = np.append(self._distance_solution_container, self._delta) # check sign self._sign_check = np.sign(self._delta * self._distance_solution_container[-1]) # contact has happened, but time step has to be reduced as initial penetration depth is too large # if (np.sign(self._dqn_solution_container[-1]) == +1) or (self._dqn_solution_container[-1] == 0) and (self._sign_check == -1) and (self._distance >= self._radial_clearance): # if (self._sign_check == -1) and (self._distance >= self._radial_clearance): if (self._sign_check == -1) and (self._delta <= 0): # beginning of contact detected, all parameters are within limits if abs(self._delta) <= self.distance_TOL: # else: self._delta0 = self._delta self.contact_detected = True self.contact_distance_inside_tolerance = True self.status = 1 # print int(self._step_num_solution_container[-1]), self.status, t, self._distance, return 1 # step back if abs(self._delta) > self.distance_TOL: self.contact_detected = True self.status = -1 # self._distance_solution_container = np.delete(self._distance_solution_container, -1) return -1 # all calculated distances are greater than tolerance and bodies are not in contact self.status = 0 # self._status_container = np.append(self._status_container, self.status) self.no_contact() # for _force_n, _force_t in zip(self._Fn_list, self._Ft_list): # _force_n.update(self._step) # _force_t.update(self._step) return 0 def contact_update(self, step, t, q): """ Function evaluates contact distance while, contact is present :return: status - status of contact 0-no contact, 2-contact """ self._step = step # print "self._step =", self._step # current contact velocity at time t self._dq_n, self._dq_t = self._contact_velocity(q) # print "self._dq_n =", self._dq_n # calculate distance between joint centers and delta (penetration) self._distance, self._delta = self._contact_geometry_GCS(q) # print "self._distance", self._distance # print "self._delta", self._delta # print "self._distance >= self._radial_clearance =", self._distance >= self._radial_clearance # print "abs(self._delta) >= self.distance_TOL =", abs(self._delta) >= self.distance_TOL # time.sleep(1) # if distance is greater than radial clearance, contact is present if self._delta <= self._delta0:#(self._distance >= self._radial_clearance) and (abs(self._delta) >= self.distance_TOL): self.status = 1 else: self.status = 0 self.contact_detected = False self.contact_distance_inside_tolerance = False # self.list_of_contact_force_objects_constructed = False self._contact_point_found = False self.initial_contact_velocity_calculated = False self.no_contact() return self.status def _get_contact_geometry_data(self, q): """ Function calculates a vector - point of contact from global coordinates to local coordinates of each body """ # 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_LCS_list = [self._t_GCS, -self._t_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 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 _contact_geometry_GCS(self, q): """ Function calculates distance between centerpoints and indentation based on radius of pin/hole :param q: :return distance_obj: distance object of calculated data in GCS """ self.u_CP_GCS_list = self._contact_geometry_CP_GCS(q) print "self.u_CP_GCS_list =", self.u_CP_GCS_list # calculate distance between axis of both bodies in revolute joint self._distance_obj = DistanceRevoluteClearanceJoint(self.u_CP_GCS_list[0], self.u_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 # contact is present and actual contact point has to be evaluated # if _distance >= self._radial_clearance and abs(_delta) >= self.distance_TOL: # print "contact is present" # unit vector in direction from one center to enother (pin to hole) self._n_GCS = self._distance_obj._distance_vector / self._distance_obj._distance # print "--------------------------------" # print "step =", self._step # print "n =", self._n # if _delta < 0 and abs(_delta) > self.distance_TOL: # print "body i =", self.u_CP_list_GCS[0] # print "body j =", self.u_CP_list_GCS[1] # print "n =", self._n # create normal list in LCS self._n_GCS_list = [-self._n_GCS, +self._n_GCS] self._n_LCS_list = [] for body_id, _normal 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=_normal, theta=_theta) # append normal to list self._n_LCS_list.append(_normal_LCS) # calculate a actual contact point in revolute clearance joint of each body in GCS # and vector of contact point in LCS self.u_P_GCS_list = [] self.u_P_LCS_list = [] # plot = False#[False, self.status==2] #False # if plot: # print "*************************" # fig = plt.gcf() # plt.xlim([0.0, 0.01]) # plt.ylim([0.0, 0.01]) # ax = plt.gca() # # ax.relim() # ax.autoscale_view(True,True,True) # ax.set_aspect('equal') self.u_P_LCS_list = [] # evaluate actual contact point in LCS of each body and in GCS for body_id, _u_CP_GCS, _u_CP_LCS, _R0 in zip(self.body_id_list, self.u_CP_GCS_list, self.u_CP_LCS_list, self.R0_list): # print "body_id =", body_id # q of a body q_body = q2q_body(q, body_id) # contact point in GCS _u_P_GCS = _u_CP_GCS + _R0 * self._n_GCS # contact point in body LCS _u_P_LCS = gcs2cm_lcs(_u_P_GCS, q_body[0:2], q_body[2]) self.u_P_LCS_list.append(_u_P_LCS) # if plot: # print "----------------------------------" # print "body =", self._parent._parent.bodies[body_id]._name # # R = q_body[0:2] # print "q_body[0:2] =", q_body[0:2] # print "joint center =", _u_CP_LCS # print "contact point GCS =", _u_P_GCS # print "contact point LCS =", _u_P_LCS # _color = np.random.rand(3) # circle=plt.Circle((_u_CP_LCS[0]+R[0],_u_CP_LCS[1]+R[1]),_R,color=_color, fill=False) # # center of axis # ax.plot(_u_CP_LCS[0], _u_CP_LCS[1], "o", color=_color) # # contact point # ax.plot(_u_P_LCS[0]+R[0], _u_P_LCS[1]+R[1], "x", color=_color) # # LCS # ax.plot(R[0], R[1], "s", color=_color) # fig.gca().add_artist(circle) # transform to 32bit float to display in opengl self.u_P_GCS_list = np.array(self.u_P_GCS_list, dtype="float32") self.u_P_LCS_list = np.array(self.u_P_LCS_list, dtype="float32") # self.u_P_GCS = np.array(_u_P_GCS, dtype="float32") # if self._step_solution_accepted: # self._u_P_solution_container.append(self.u_P_list_LCS.flatten()) # if plot: # plt.grid() # plt.show() # fig.savefig('testing.png') return _distance, _delta def _contact_velocity(self, q): """ Function evaluates relative contact velocity at contact point in revolute clearance joint. :param q: array of coordinates (r, theta) and velocities (dR, dhete=omega) of MBD system """ dr_P = [] print "self.u_P_LCS_list =", self.u_P_LCS_list, type(self.u_P_LCS_list) for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list): dR = q2dR_i(q, body_id) theta = q2theta_i(q, body_id) # dtheta - omega dtheta = q2dtheta_i(q, body_id) # point velocity dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P) # add to list dr_P.append(dr_P_body) _dq = dr_P[0] - dr_P[1] # relative contact velocity # normal direction _dq_n = np.dot(_dq, self._n_GCS) # tangent direction _dq_t = np.dot(_dq, self._t_GCS) return _dq_n, _dq_t def solve(self, t, q): """ Calculate contact parameters returns: """ # print "solve() @ RCJ()" # print "Ry_i =", q[1] # print "Ry_j =", q[4] # calculate coordinates of contact point from global coordinates in local coordinates of each body in contact if not self._contact_point_found: self._get_contact_geometry_data(q) self._contact_point_found = True else: pass # self._distance, self._delta = self._contact_geometry_GCS(q) # print "contact update" # print "self._contact_point_found =", self._contact_point_found # kinematic properties of contact point # initial contact velocity if not self.initial_contact_velocity_calculated: self._dq0_n, self._dq0_t = self._contact_velocity(q) self.contact_model.set_dq0(self._dq0_n, self._dq0_t) self.initial_contact_velocity_calculated = True # if self.contact_detected: self._distance, self._delta = self._contact_geometry_GCS(q) # print "self._delta =", self._delta # print "self._dq0_n, self._dq0_t =", self._dq0_n, self._dq0_t # current contact velocity at time t self._dq_n, self._dq_t = self._contact_velocity(q) # print "self._dq_n, self._dq_t =", self._dq_n, self._dq_t # time.sleep(100) # current contact velocity at time t # self._dq_n, self._dq_t = self._contact_velocity(q) if self._type.lower() in self._types:#== "general" or self._type.lower() == "revolute clearance joint" or self._type.lower() == "contact sphere-sphere":#ECF-N self._solve_ECF_N(t, q, self._delta, self._dq_n, self._dq_t)#self._delta else: raise ValueError, "Contact type not correct!"