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
