def can_screw(self, target_hole_info, const_dict):
print("\n")
can_screw = False
hole_end = target_hole_info["endCoordinate"]
hole_end_tr = tf.translation_from_matrix(hole_end)
hole_end_quat = tf.quaternion_from_matrix(hole_end)
print(hole_end_tr)
for const_name, const_info in const_dict.items():
target_init_parent = target_hole_info["name"].split("-")[0]
const_init_parent = const_name.split("_")[0]
if target_init_parent != const_init_parent:
if const_info["name"] != target_hole_info["name"] and "hole" in const_name and const_info["contactWith"] is None:
hole_entry = const_info["entryCoordinate"]
hole_entry_tr = tf.translation_from_matrix(hole_entry)
dist = np.linalg.norm(hole_end_tr - hole_entry_tr)
print(" {} {} {}".format(const_name, hole_entry_tr, dist))
if dist <= 0.005:
hole_entry_quat = tf.quaternion_from_matrix(hole_entry)
hole_entry_z_axis = utils.get_transformed_zAxis(hole_entry_quat)
hole_end_z_axis = utils.get_transformed_zAxis(hole_end_quat)
degree = self.axis_diff(hole_entry_z_axis, hole_end_z_axis)
if degree <= 10:
can_screw = True
break
return can_screw
def update_real_const_pose(self):
if self.real_pose_mat is None:
rospy.logwarn("Real pose of '{}' is not detected!".format(self.referencePart))
pass
else:
for const_name in self.assemConsts.keys():
const = self.assemConsts[const_name]
const_end_pose = const["endCoordinate"]
real_const_end_pose = tf.concatenate_matrices(self.real_pose_mat, const_end_pose)
const["real_endCoordinate"] = real_const_end_pose
const_quat = tf.quaternion_from_matrix(real_const_end_pose)
const["real_zAxis"] = utils.get_transformed_zAxis(const_quat)
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 get_AsmPose_by_HolePin(self, ref_obj, ref_const_names, move_obj,
move_const_names):
target_tr = np.array([0, 0, 0])
target_quat = np.array([0, 0, 0, 1])
ref_axis = np.array([0, 0, -1])
criterion = []
success = False
ref_consts = [ref_obj.assemConsts[const] for const \
in ref_const_names]
move_consts = [move_obj.assemConsts[const] for const \
in move_const_names]
criterion = [self.HolePin_criterion(ref, move) for (ref, move) \
in zip(ref_consts, move_consts)]
if False in criterion:
pprint(ref_consts)
pprint(move_consts)
rospy.logwarn(
"Hole and pin's specs are not satispying given citerion")
pass
else:
ref_coors = [const[cri+"Coordinate"] for (const, cri) \
in zip(ref_consts, criterion)]
ref_quats = [tf.quaternion_from_matrix(coor) for coor in ref_coors]
ref_trs = [tf.translation_from_matrix(coor) for coor in ref_coors]
ref_zs = [utils.get_transformed_zAxis(quat) for quat in ref_quats]
ref_z_diffs = [
utils.zAxis_difference(ref_zs[0], zAxis) for zAxis in ref_zs
]
ref_tr_diffs = [tr - ref_trs[0] for tr in ref_trs]
move_coors = [const[cri+"Coordinate"] for (const, cri) \
in zip(move_consts, criterion)]
move_quats = [
tf.quaternion_from_matrix(coor) for coor in move_coors
]
move_trs = [
tf.translation_from_matrix(coor) for coor in move_coors
]
move_zs = [
utils.get_transformed_zAxis(quat) for quat in move_quats
]
move_z_diffs = [
utils.zAxis_difference(move_zs[0], zAxis) for zAxis in move_zs
]
move_tr_diffs = [tr - move_trs[0] for tr in move_trs]
is_same_z_diffs = [np.allclose(ref_z_diff, move_z_diff, rtol=0.005, atol=0.005) \
for (ref_z_diff, move_z_diff) in zip(ref_z_diffs, move_z_diffs)]
if not is_same_z_diffs:
print(ref_z_diffs)
print(move_z_diffs)
print(is_same_z_diffs)
rospy.logwarn(
"Target hole and pin's direction is not proper for assembly!"
)
pass
else:
if len(ref_consts) > 1:
move_quat_inv1 = tf.quaternion_inverse(move_quats[0])
temp_quat = tf.quaternion_multiply(ref_quats[0],
move_quat_inv1)
ref_axis = ref_zs[0]
move2_tr_from_ref1_temp = \
utils.get_translated_origin([0,0,0], temp_quat, move_tr_diffs[1])[:3, 3]
move2_tr_from_ref1_temp_re = \
utils.get_translated_origin([0,0,0], tf.quaternion_inverse(ref_quats[0]), move2_tr_from_ref1_temp)[:3, 3]
ref2_tr_from_ref1_temp = \
utils.get_translated_origin([0,0,0], tf.quaternion_inverse(ref_quats[0]), ref_tr_diffs[1])[:3, 3]
grad_move2_from_ref1_temp = \
np.arctan2(move2_tr_from_ref1_temp_re[1], move2_tr_from_ref1_temp_re[0])
grad_ref2_from_ref1_temp = \
np.arctan2(ref2_tr_from_ref1_temp[1], ref2_tr_from_ref1_temp[0])
temp_grad1 = tf.quaternion_about_axis(
grad_move2_from_ref1_temp, ref_axis)
temp_grad1_inv = tf.quaternion_inverse(temp_grad1)
temp_grad2 = tf.quaternion_about_axis(
grad_ref2_from_ref1_temp, ref_axis)
target_quat = tf.quaternion_multiply(
temp_grad1_inv, temp_quat)
target_quat = tf.quaternion_multiply(
temp_grad2, target_quat)
target_tr = utils.get_translated_origin(
ref_trs[0], target_quat, -move_trs[0])[:3, 3]
else:
ref_axis = ref_zs[0]
move_quat_inv = tf.quaternion_inverse(move_quats[0])
target_quat = tf.quaternion_multiply(
ref_quats[0], move_quat_inv)
target_tr = utils.get_translated_origin(
ref_trs[0], target_quat, -move_trs[0])[:3, 3]
if "pin" in ref_const_names[0]:
ref_axis = -ref_zs[0]
else:
ref_axis = ref_zs[0]
edit_quats = [tf.quaternion_multiply(target_quat, move_quat) \
for move_quat in move_quats]
edit_zs = [
utils.get_transformed_zAxis(quat) for quat in edit_quats
]
edit_quat_mat = tf.quaternion_matrix(target_quat)
edit_coors = [tf.concatenate_matrices(edit_quat_mat, coor) \
for coor in move_coors]
edit_trs = [
tf.translation_from_matrix(coor) for coor in edit_coors
]
edit_tr_diffs = [tr - edit_trs[0] for tr in edit_trs]
edit_tr_min = self.get_min_vector_from_ref(
edit_zs, edit_trs, edit_tr_diffs)
ref_tr_min = self.get_min_vector_from_ref(
ref_zs, ref_trs, ref_tr_diffs)
is_same_arange = [np.allclose(ref, move, rtol=0.05, atol=0.05) \
for (ref, move) in zip(ref_tr_min, edit_tr_min)]
arange_success = all(is_same_arange)
stuck_success = self.check_const_mating(
ref_consts, move_consts, target_tr, target_quat)
success = arange_success and stuck_success
return target_tr, target_quat, ref_axis, criterion, success