def _contact_geometry_GCS(self, q): """ Function evaluates contact geometry in GCS Function evaluates: self._n_GCS: normal of contact in GCS: self._t_GCS: tangent of contact in GCS self.node_GCS: contact point in GCS (on body i and on body j - unverified) :param q: :return delta: :return distance: """ # print "_contact_geometry_GCS()" # evaluate coordinates in GCS of mid frame section and pin center pin_CP_GCS, slot_CP_GCS, slot_frame_nodes_GCS, slot_frame_normals_GCS, slot_frame_tangents_GCS = self._contact_geometry_CP_GCS(q) # center points in GCS self.u_CP_GCS_list = [pin_CP_GCS, slot_CP_GCS[0], slot_CP_GCS[1]] # print "self.u_CP_GCS_list =", self.u_CP_GCS_list # time.sleep(100) # evaluate position of pin in slot: # section jP # section jPjR # section jR self.pin_in_section_jP, self.pin_in_section_jPjR, self.pin_in_section_jR = self._check_pin_position(slot_frame_nodes_GCS, pin_CP_GCS) # evaluate contact distance and penetration depth based on in what part of the slot is the pin if self.pin_in_section_jPjR: # define empty list to store 2 distance object # distance of pin CP to edge AB # distance of pin CP to edge CD distance_obj_list = [] # print "--------------------" for i, (_n, _t) in enumerate(zip(slot_frame_normals_GCS, slot_frame_tangents_GCS)): _distance_obj = DistanceLineNode(u_iP=slot_frame_nodes_GCS[2*i, :], u_jP=pin_CP_GCS, normal=_n, u_iR=slot_frame_nodes_GCS[2*i+1, :], tangent=_t) distance_obj_list.append(_distance_obj) # print "i =", i, "_distance =", _distance_obj._distance # get distance object with min distance attribute value self._distance_obj = min(distance_obj_list, key=attrgetter('_distance')) # add edge id to object to have info with which edge of flat slot surface is contact with a pin self._distance_obj.edge_id = distance_obj_list.index(self._distance_obj) # get distance attribute from distance object _distance = self._distance_obj._distance # evaluated penetration depth _delta = self._evaluate_delta_flat_section(_distance) # print "_delta =", _delta # tangent in GCS # pprint(vars(self._distance_obj)) # time.sleep(100) else: if self.pin_in_section_jP: # calculate distance between axis of both bodies in half revolute clearance joint at point Pj _slot_CP_GCS = slot_CP_GCS[0] elif self.pin_in_section_jR: # calculate distance between axis of both bodies in half revolute clearance joint at point Rj _slot_CP_GCS = slot_CP_GCS[1] else: raise ValueError, "Contact point not in section!" # print "pin_CP_GCS =", pin_CP_GCS # print "_slot_CP_GCS =", _slot_CP_GCS self._distance_obj = DistanceRevoluteClearanceJoint(_slot_CP_GCS, pin_CP_GCS, parent=self) # penetration depth is difference between nominal radial clearance and actual calculated clearance at time t _distance = self._distance_obj._distance # print "_distance =", _distance _delta = self._evaluate_delta_cylindrical_section(_distance) # print "_delta =", _delta # if penetration depth is below small value, contact is present and contact geometry parameters are evaluated # if _delta <= self._delta0 and not self._contact_point_found: # if self.pin_in_section_jPjR: # # contact normal in GCS # self._n_GCS = self._distance_obj.normal return _distance, _delta
class PinSlotClearanceJointLinear(Contact): """ classdocs """ __id = itertools.count(0) def __init__(self, _type, body_id_i, body_id_j, u_iP, u_jP, u_jR, R0_i, h0_jP, L=None, h0_jR=None, properties_dict=[], parent=None): """ :param _type: :param body_id_i: id of a body - pin :param body_id_j: id of a body - slot :param u_iP: point vector to center of a pin on body i (in body CAD CS) :param u_jP: point vector to start point center of the slot on body j (in body CAD CS) :param u_jR: point vector to end point center of the slot on body j (in body CAD CS) :param R0_i: radius of a pin on body i :param h0_jP: width of a slot at start point center on body i :param h0_jR: width of a slot at end point center on body j :return: """ # number self.contact_id = self.__id.next() self._name = "Pin_Slot_Clearance_Joint_"+str(self.contact_id) super(PinSlotClearanceJointLinear, self).__init__(_type, body_id_i, body_id_j, name=self._name, properties_dict=properties_dict, parent=parent) # parent self._parent = parent # type of contact self._type = "pin-slot clearance joint linear" # contact model # add all to list of CP - center points # 1 - body i (pin) center # 2 - body j (slot) center uP # 3 - body j (slot) center uR self.u_CP_LCS_list = [] # body i # body i (pin) geometry parameters if self._parent is not None: if hasattr(self._parent._parent.bodies[body_id_i], "CM_CAD_LCS"): self.u_iP = cad2cm_lcs(u_iP, self._parent._parent.bodies[body_id_i].CM_CAD_LCS, 0) else: self.u_iP = u_iP self.u_CP_LCS_list.append(self.u_iP) # radius of a pin self.R0_i = R0_i # list of radii of pin and of slot self.R0_list = [self.R0_i, h0_jP / 2.] # body j # body j (slot) geometry data # predefine empty list of slot centers self.u_CP_slot_LCS = [] if self._parent is not None: if hasattr(self._parent._parent.bodies[body_id_j], "CM_CAD_LCS"): self.u_jP = cad2cm_lcs(u_jP, self._parent._parent.bodies[body_id_j].CM_CAD_LCS, 0) self.u_jR = cad2cm_lcs(u_jR, self._parent._parent.bodies[body_id_j].CM_CAD_LCS, 0) else: # vector to first point of center of a slot in self.u_jP = u_jP # vector to second point of center of a slot self.u_jR = u_jR # append to centers list of a slot self.u_CP_slot_LCS.append(self.u_jP) self.u_CP_slot_LCS.append(self.u_jR) # append to centers list self.u_CP_LCS_list.append(self.u_jP) self.u_CP_LCS_list.append(self.u_jR) # in GCS self.u_CP_GCS_list = [np.zeros(2), np.zeros(2), np.zeros(2)] # body ids for three point (1 point pin, 2 points slot) self.body_id_extended_list = [self.body_id_i, self.body_id_j, self.body_id_j] # width of a slot at first point of center of a slot self.h0_jP = h0_jP # width of a slot at second point of center of a slot if h0_jR is None: self.h0_jR = h0_jP else: self.h0_jR = h0_jR # clearance self._radial_clearance = (self.h0_jR / 2.) - self.R0_i # tangent of slot edge self.t_LCS = self.u_jR - self.u_jP # length of flat edge of slot self.L = np.linalg.norm(self.t_LCS, ord=2) # unit vector of direction of edge self.t_LCS_unit_vector = self.t_LCS / self.L # list of tangents in LCS or edge vectors self.t_edge_slot_LCS = [] self.t_edge_slot_LCS.append(self.t_LCS_unit_vector) self.t_edge_slot_LCS.append(-self.t_LCS_unit_vector) # normal unit vector self.n_LCS_unit_vector = t2n(self.t_LCS_unit_vector) # list of normals in LCS of flat edges in slot- section jPjR self.n_edge_slot_LCS = [] self.n_edge_slot_LCS.append(self.n_LCS_unit_vector) self.n_edge_slot_LCS.append(-self.n_LCS_unit_vector) # create slot frame geometry for flat surface self.frame_nodes_LCS = self._create_slot_frame_flat_surface(self.u_jP, self.u_jR, self.n_LCS_unit_vector, self.t_LCS_unit_vector, self.h0_jP) # list of markers self.markers = self._create_markers() # reset additional parameters before simulation self.reset() 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): # node coordinates _node = np.array(np.append(u_P, self.z_dim), dtype="float32") # get correct body object if i == 0: body = self.body_list[0] if i == 1 or i == 2: body = self.body_list[1] # 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 contact markers markers.append(_marker) return markers def reset(self): """ Function is used to reset simulation parameters before new simulation run :return: """ # contact status # left half cylinder self.pin_in_section_jP = False # flat section self.pin_in_section_jPjR = False # right half cylinder self.pin_in_section_jR = False # predefined list of contact point on each body (one contact point on a body) self.u_P_list_LCS = np.array([np.zeros(2), np.zeros(2)]) def check_for_contact(self, q): """ Check for contact :param q: :return: """ # print "check_for_contact()" self._distance, self._delta = self._contact_geometry_GCS(q) # print "self._delta =", self._delta # print "self._delta@check_for_contact() =", 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): # print "(self._sign_check == -1) and (self._delta <= 0) =", (self._sign_check == -1) and (self._delta <= 0) if (self._sign_check == -1) and (self._delta <= 0): # print "sign and delta" # 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 "---------------------------------------------" # print "contact detected", "L =", self.L # print "self._delta =", self._delta # print "DA =", self._pin_distance_to_DA_obj._distance # print "BC =", self._pin_distance_to_BC_obj._distance # print "section jPjR =", self._pin_distance_to_DA_obj._distance < self.L, self._pin_distance_to_BC_obj._distance < self.L # print "section jP =", self._pin_distance_to_DA_obj._distance < self.L, self._pin_distance_to_BC_obj._distance >= self.L # print "section jR =", self._pin_distance_to_DA_obj._distance >= self.L, self._pin_distance_to_BC_obj._distance < self.L # print "t =", self.t, "step =", self._step, "pin in: ", # if self.pin_in_section_jPjR: # print "flat section" # if self.pin_in_section_jP: # print "section jP" # if self.pin_in_section_jR: # print "section jR" # print "contact point =", # 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() return 0 def contact_update(self, step, t, q): """ :param step: :param t: :param q: :return: """ # print "contact_update =" # update time step self._step = step # current contact velocity at time t self._dq_n, self._dq_t = self._contact_velocity(q) # calculate distance between joint centers and delta (penetration) self._distance, self._delta = self._contact_geometry_GCS(q) # print "self._delta@ =", self._delta, # print "self._delta0 =", self._delta0 # print self._delta <= self._delta0 # print "self._contact_point_found =", self._contact_point_found # time.sleep(1) if self._delta <= self._delta0:#(self._distance >= self._radial_clearance) and (abs(self._delta) >= self.distance_TOL): # print "exe??" 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 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 evaluates contact geometry in GCS Function evaluates: self._n_GCS: normal of contact in GCS: self._t_GCS: tangent of contact in GCS self.node_GCS: contact point in GCS (on body i and on body j - unverified) :param q: :return delta: :return distance: """ # print "_contact_geometry_GCS()" # evaluate coordinates in GCS of mid frame section and pin center pin_CP_GCS, slot_CP_GCS, slot_frame_nodes_GCS, slot_frame_normals_GCS, slot_frame_tangents_GCS = self._contact_geometry_CP_GCS(q) # center points in GCS self.u_CP_GCS_list = [pin_CP_GCS, slot_CP_GCS[0], slot_CP_GCS[1]] # print "self.u_CP_GCS_list =", self.u_CP_GCS_list # time.sleep(100) # evaluate position of pin in slot: # section jP # section jPjR # section jR self.pin_in_section_jP, self.pin_in_section_jPjR, self.pin_in_section_jR = self._check_pin_position(slot_frame_nodes_GCS, pin_CP_GCS) # evaluate contact distance and penetration depth based on in what part of the slot is the pin if self.pin_in_section_jPjR: # define empty list to store 2 distance object # distance of pin CP to edge AB # distance of pin CP to edge CD distance_obj_list = [] # print "--------------------" for i, (_n, _t) in enumerate(zip(slot_frame_normals_GCS, slot_frame_tangents_GCS)): _distance_obj = DistanceLineNode(u_iP=slot_frame_nodes_GCS[2*i, :], u_jP=pin_CP_GCS, normal=_n, u_iR=slot_frame_nodes_GCS[2*i+1, :], tangent=_t) distance_obj_list.append(_distance_obj) # print "i =", i, "_distance =", _distance_obj._distance # get distance object with min distance attribute value self._distance_obj = min(distance_obj_list, key=attrgetter('_distance')) # add edge id to object to have info with which edge of flat slot surface is contact with a pin self._distance_obj.edge_id = distance_obj_list.index(self._distance_obj) # get distance attribute from distance object _distance = self._distance_obj._distance # evaluated penetration depth _delta = self._evaluate_delta_flat_section(_distance) # print "_delta =", _delta # tangent in GCS # pprint(vars(self._distance_obj)) # time.sleep(100) else: if self.pin_in_section_jP: # calculate distance between axis of both bodies in half revolute clearance joint at point Pj _slot_CP_GCS = slot_CP_GCS[0] elif self.pin_in_section_jR: # calculate distance between axis of both bodies in half revolute clearance joint at point Rj _slot_CP_GCS = slot_CP_GCS[1] else: raise ValueError, "Contact point not in section!" # print "pin_CP_GCS =", pin_CP_GCS # print "_slot_CP_GCS =", _slot_CP_GCS self._distance_obj = DistanceRevoluteClearanceJoint(_slot_CP_GCS, pin_CP_GCS, parent=self) # penetration depth is difference between nominal radial clearance and actual calculated clearance at time t _distance = self._distance_obj._distance # print "_distance =", _distance _delta = self._evaluate_delta_cylindrical_section(_distance) # print "_delta =", _delta # if penetration depth is below small value, contact is present and contact geometry parameters are evaluated # if _delta <= self._delta0 and not self._contact_point_found: # if self.pin_in_section_jPjR: # # contact normal in GCS # self._n_GCS = self._distance_obj.normal return _distance, _delta def _contact_geometry_LCS(self, q): """ Function evaluates contact geometry parameters in body LCS based on contact geometry in GCS Function evaluates: self._n_LCS_list: normal of contact in body LCS: self._t_LCS_list: tangent of contact in body LCS self.u_P_LCS_list: contact point in body LCS """ # print "_contact_geometry_LCS()" if self.pin_in_section_jPjR: # contact point on body i - pin # in LCS self.u_P_LCS_list[0] = self.u_iP_LCS = Ai_ui_P_vector(-self._n_GCS * self.R0_i, q2theta_i(q, self.body_id_i)) # in GCS self.u_P_GCS_list[0] = self.u_iP_GCS = q2R_i(q, self.body_id_i) + self.u_iP_LCS # contact point on body j in GCS self.u_P_GCS = self.u_P_GCS_list[1] = self.u_jP_GCS = self._distance_obj.contact_point_on_line_GCS() # time.sleep(10) # contact point on body j in LCS self.u_P_LCS_list[1] = self.u_jP_LCS = gcs2cm_lcs(self.u_P_GCS, q2R_i(q, self.body_id_j), q2theta_i(q, self.body_id_j)) # in GCS # self.u_iP_GCS = u_P_lcs2gcs(self.u_iP_LCS, q, self.body_id_i) + (-self._n_GCS) * self.R0_i # print "self.u_iP_GCS =", self.u_iP_GCS # get normal and tangent of each body for i, (sign, body_id) in enumerate(zip([-1, +1], self.body_id_list)): theta = q2theta_i(q, body_id) # evaluate body normal in body LCS self._n_LCS_list[i] = gcs2cm_lcs(sign * self._n_GCS, theta=theta) # evaluate body tangent in body LCS self._t_LCS_list[i] = gcs2cm_lcs(sign * self._t_GCS, theta=theta) elif self.pin_in_section_jP or self.pin_in_section_jR: # create normal list in LCS self._n_GCS_list = [self._n_GCS, -self._n_GCS] for i, (body_id, _normal) in enumerate(zip(self.body_id_list, self._n_GCS_list)): # normal in LCS _theta = q2theta_i(q, body_id) # _normal_LCS = Ai_ui_P_vector(_normal, _theta) _normal_LCS = uP_gcs2lcs(_normal, _theta) # append normal to list self._n_LCS_list[i] = _normal_LCS if self.pin_in_section_jP: _u_CP_LCS_list = [self.u_CP_LCS_list[0], self.u_CP_LCS_list[1]] _u_CP_GCS_list = [self.u_CP_GCS_list[0], self.u_CP_GCS_list[1]] if self.pin_in_section_jR: _u_CP_LCS_list = [self.u_CP_LCS_list[0], self.u_CP_LCS_list[2]] _u_CP_GCS_list = [self.u_CP_GCS_list[0], self.u_CP_GCS_list[2]] # evaluate actual contact point in LCS of each body and in GCS for i, (body_id, _u_CP_LCS, _R0) in enumerate(zip(self.body_id_list, _u_CP_LCS_list, self.R0_list)): # R of body R = q2R_i(q, body_id) # theta of body theta = q2theta_i(q, body_id) # contact point in body LCS _u_P_LCS = _u_CP_LCS self.u_P_LCS_list[i] = _u_P_LCS# + _R0 * self._n_GCS # contact point in GCS self.u_P_GCS_list[i] = R + Ai_ui_P_vector(_u_CP_LCS, theta) + _R0 * self._n_GCS # print "self.u_P_GCS_list =", self.u_P_GCS_list def _evaluate_delta_flat_section(self, distance): """ Evaluate penetration depth - delta for contact between pin and flat surface of the slot :param distamce: :return: """ delta = distance - self.R0_i#((self.h0_jP - 2 * ) / 2.) - return delta def _evaluate_delta_cylindrical_section(self, distance): """ Evaluate penetration depth - delta for contact between pin and cylindrical surface of the slot :param distamce: :return: """ # print "_evaluate_delta_cylindrical_section()" # print "distance =", distance # print "self._radial_clearance =", self._radial_clearance delta = self._radial_clearance - distance return delta def _check_pin_position(self, frame_nodes_GCS, pin_CP_GCS): """ Function eveluates where in slot is pin located to use correct equation for evaluation of penetration depth :param frame_nodes_GCS: position of slot frame nodes in GCS :param pin_CP_GCS: position of pin center in GCS :return: """ # print "_check_pin_position()" # set default values pin_in_section_jP = pin_in_section_jPjR = pin_in_section_jR = False # distance of pin center to line DA and BC # node A u_Aj = frame_nodes_GCS[0, :] # node B u_Bj = frame_nodes_GCS[1, :] # node C u_Cj = frame_nodes_GCS[2, :] # node D u_Dj = frame_nodes_GCS[3, :] # DA self._pin_distance_to_DA_obj = DistanceLineNode(u_Aj, pin_CP_GCS, u_iR=u_Dj, parent=self) # print "_pin_distance_to_DA_obj =", _pin_distance_to_DA_obj._distance # BC self._pin_distance_to_BC_obj = DistanceLineNode(u_Cj, pin_CP_GCS, u_iR=u_Bj, parent=self) # print "_pin_distance_to_BC_obj =", _pin_distance_to_BC_obj._distance if self._pin_distance_to_DA_obj._distance < self.L and self._pin_distance_to_BC_obj._distance < self.L: pin_in_section_jPjR = True # print "pin in jPjR" if self._pin_distance_to_DA_obj._distance < self.L and self._pin_distance_to_BC_obj._distance >= self.L: pin_in_section_jP = True # print "pin in jP" if self._pin_distance_to_DA_obj._distance >= self.L and self._pin_distance_to_BC_obj._distance < self.L: pin_in_section_jR = True # print "pin in jR" # section from (u_)jP to (u_)jR # if (frame_nodes_GCS[0, 0] < pin_CP_GCS[0] < frame_nodes_GCS[1, 0]) and (frame_nodes_GCS[0, 1] < pin_CP_GCS[1] < frame_nodes_GCS[2, 1]): # pin_in_section_jPjR = True # # print "pin in jPjR" # # # half-circlurar section at (u_)jP # if (pin_CP_GCS[0] <= max(frame_nodes_GCS[0, 0], frame_nodes_GCS[3, 0])) and (pin_CP_GCS[1] <= max(frame_nodes_GCS[0, 1], frame_nodes_GCS[3, 1])): # # print "pin_CP_GCS[0] =", pin_CP_GCS[0] # # print "max x =", max(frame_nodes_GCS[0, 0], frame_nodes_GCS[3, 0]) # # print "pin_CP_GCS[1] =", pin_CP_GCS[1] # # print "max y =", max(frame_nodes_GCS[0, 1], frame_nodes_GCS[3, 1]) # pin_in_section_jP = True # # print "pin in jP" # # # half-circlurar section at (u_)jR # if (pin_CP_GCS[0] >= min(frame_nodes_GCS[1, 0], frame_nodes_GCS[2, 0])) and (pin_CP_GCS[1] >= min(frame_nodes_GCS[1, 1], frame_nodes_GCS[2, 1])): # pin_in_section_jR = True # # print "pin in jR" return pin_in_section_jP, pin_in_section_jPjR, pin_in_section_jR def _contact_geometry_CP_GCS(self, q): """ :param q: :return: """ # print "_contact_geometry_CP_GCS()" # calculate position of pin/hole centers of each body in GCS # this is a list of CP - center points # 1 - u_CP_list_GCS = [] # center points in GCS # for uP, id in zip(self.u_CP_LCS_list, self.body_id_extended_list): # uP_gcs = q2R_i(q, id) + Ai_ui_P_vector(uP, q2theta_i(q, id)) # print "uP_gcs =", uP_gcs # print "_update_slot_frame_flat_surface" # slot frame nodes in GCS frame_nodes_GCS, frame_normals_GCS, frame_tangents_GCS = self._slot_frame_flat_surface_GCS(q) # print "frame_nodes_GCS =", frame_nodes_GCS # pin center in GCS pin_CP_GCS = self._pin_GCS(q) # print "pin_CP_GCS =", pin_CP_GCS # slot center points in GCS slot_CP_GCS = self._slot_GCS(q) return pin_CP_GCS, slot_CP_GCS, frame_nodes_GCS, frame_normals_GCS, frame_tangents_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 _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 _create_slot_frame_flat_surface(self, uPj, uRj, n, t, hPj, hRj=None): """ Direction of tangent uPj->hRj Direction of normal uPj->T1 Frame shape: A------------------>B | | uPj uRj | | D<------------------C :param q: a vector (numpy array) of positions :return frame_nodes_LCS: a matrix (numpy array) of frame nodes """ # 4 frame nodes in a matrix frame_nodes_LCS = np.array([uPj - n*hPj*.5, #0 A uRj - n*hPj*.5, #1 B uRj + n*hPj*.5, #2 C uPj + n*hPj*.5], #3 D dtype='float32') return frame_nodes_LCS def _slot_frame_flat_surface_GCS(self, q): """ Function updates and calculates position of slot frame in global coordinates - GCS :param q: :return: """ # nodes in GCS frame_nodes_GCS = q2R_i(q, self.body_id_j) + Ai_ui_P_vector(self.frame_nodes_LCS, q2theta_i(q, self.body_id_j)) # normals in GCS frame_normals_GCS = Ai_ui_P_vector(np.array(self.n_edge_slot_LCS), q2theta_i(q, self.body_id_j)) # tangents in GCS frame_tangents_GCS = Ai_ui_P_vector(np.array(self.t_edge_slot_LCS), q2theta_i(q, self.body_id_j)) return frame_nodes_GCS, frame_normals_GCS, frame_tangents_GCS