def attach_obj(self, move_obj, rel_tr, rel_quat, subassembly_name): self.attach_cm(move_obj.cm, move_obj.assem_mass, rel_tr, rel_quat) self.attach_consts(move_obj.assemConsts, rel_tr, rel_quat) self.attach_dict(move_obj.assemDict, rel_tr, rel_quat, move_obj.referencePart) self.assemXML = xmltodict.unparse(self.assemDict) self.assem_mass += move_obj.assem_mass rel_tf = utils.get_tf_matrix(rel_tr, rel_quat) for comp_name, comp in move_obj.components.items(): comp_tr = comp["tr"] comp_quat = comp["quat"] comp_mass = comp["mass"] comp_tf = utils.get_tf_matrix(comp_tr, comp_quat) new_tf = tf.concatenate_matrices(rel_tf, comp_tf) new_tr = tf.translation_from_matrix(new_tf) new_quat = tf.quaternion_from_matrix(new_tf) self.components[comp_name] = { "tr": new_tr, "quat": new_quat, "mass": comp_mass } self.subassem_components[self.obj_name]["mass"] = self.assem_mass self.subassem_components[move_obj.assem_name] = { "tr": rel_tr, "quat": rel_quat, "mass": move_obj.assem_mass } self.assem_name = subassembly_name
def attach_cm(self, move_cm, move_mass, rel_tr, rel_quat): move_cm_mat = tf.translation_matrix(move_cm) rel_tf = utils.get_tf_matrix(rel_tr, rel_quat) new_move_cm_mat = tf.concatenate_matrices(rel_tf, move_cm_mat) new_move_cm = tf.translation_from_matrix(new_move_cm_mat) inter_cm = (new_move_cm-self.cm) / (self.assem_mass+move_mass) * move_mass new_cm = self.cm + inter_cm self.cm = new_cm
def get_tfMat_from_pose(self, pose): tr = np.array( [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]) quat = np.array([ pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w ]) tf_mat = utils.get_tf_matrix(tr, quat) return tf_mat
def attach_consts(self, move_const, rel_tr, rel_quat): rel_tf = utils.get_tf_matrix(rel_tr, rel_quat) const = copy.deepcopy(move_const) for key in const.keys(): const[key]["parent"] = self.referencePart coordinate_keys = [ck for ck in const[key].keys() if "Coordinate" in ck] for coordinate_key in coordinate_keys: const[key][coordinate_key] = tf.concatenate_matrices(rel_tf, const[key][coordinate_key]) if "real_zAxis" in const[key].keys(): const[key]["real_endCoordinate"] = tf.concatenate_matrices(self.real_pose_mat, const[key]["endCoordinate"]) const_quat = tf.quaternion_from_matrix(const[key]["real_endCoordinate"]) const[key]["real_zAxis"] = utils.get_transformed_zAxis(const_quat) self.assemConsts[key] = const[key]
def update_const_tf(self, ref_obj, const_name, tf_name, xyz, quat): obj = copy.deepcopy(ref_obj) target_const = obj.assemConsts[const_name] cri = tf_name.split("_")[-1] re_coord = utils.get_tf_matrix(xyz, quat) target_const[cri + "Coordinate"] = re_coord const_length = target_const["length"] if cri == "end": target_const["entryCoordinate"] = utils.get_translated_origin( xyz, quat, [0, 0, -const_length]) else: target_const["endCoordinate"] = utils.get_translated_origin( xyz, quat, [0, 0, const_length]) if const_name + "_spare" in obj.assemConsts.keys(): spare_const = obj.assemConsts[const_name + "_spare"] spare_xyz = xyz rot_pi_x = tf.quaternion_from_euler(m.pi, 0, 0) spare_quat = tf.quaternion_multiply(rot_pi_x, quat) spare_const["endCoordinate"] = utils.get_translated_origin( spare_xyz, spare_quat, [0, 0, const_length]) spare_const["entryCoordinate"] = utils.get_translated_origin( spare_xyz, spare_quat, [0, 0, -const_length]) return obj
def check_const_mating(self, ref_consts, move_consts, tr, quat): tf_mat = utils.get_tf_matrix(tr, quat) for ref_const, move_const in zip(ref_consts, move_consts): ref_const_copy = copy.deepcopy(ref_const) move_const_copy = copy.deepcopy(move_const) move_entry = move_const_copy["entryCoordinate"] move_end = move_const_copy["endCoordinate"] move_const_copy["entryCoordinate"] = tf.concatenate_matrices( tf_mat, move_entry) move_const_copy["endCoordinate"] = tf.concatenate_matrices( tf_mat, move_end) ref_type = ref_const["type"] move_type = move_const["type"] if ref_type == "hole": hole_const = ref_const_copy pin_const = move_const_copy else: hole_const = move_const_copy pin_const = ref_const_copy hole_entry = hole_const["entryCoordinate"] hole_end = hole_const["endCoordinate"] pin_entry = pin_const["entryCoordinate"] pin_end = pin_const["endCoordinate"] hole_entry_tr = tf.translation_from_matrix(hole_entry) hole_end_tr = tf.translation_from_matrix(hole_end) pin_entry_tr = tf.translation_from_matrix(pin_entry) pin_end_tr = tf.translation_from_matrix(pin_end) ref_axis = tf.unit_vector((hole_end_tr - hole_entry_tr).round(6)) eps = 1e-03 ref_axis[np.abs(ref_axis) < eps] = 0 inter_entry_diff = (hole_entry_tr - pin_entry_tr).round(6) #inter_entry_diff[np.abs(inter_entry_diff) < eps] = 0 inter_end_diff = (hole_end_tr - pin_end_tr).round(6) #inter_end_diff[np.abs(inter_end_diff) < eps] = 0 entry_end_diff = (pin_end_tr - hole_entry_tr).round(6) #entry_end_diff[np.abs(entry_end_diff) < eps] = 0 print("\n---") print("parent: {}, child: {}".format(ref_const["name"], move_const["name"])) print(inter_entry_diff) print(inter_end_diff) print(entry_end_diff) '''if np.linalg.norm(inter_entry_diff) == 0. or np.array_equal(np.sign(inter_entry_diff), ref_axis): pass else: print("1") return False if pin_const["feature"] == "screw": continue else: if np.array_equal(np.sign(entry_end_diff), ref_axis): pass else: print("2") return False if np.linalg.norm(inter_end_diff) == 0. or np.array_equal(np.sign(inter_end_diff), ref_axis): pass else: print("3") return False''' return True