# print("No marker found in this frame !")
return [], [], color_frame
if __name__ == '__main__':
from Grasping_Franka.Driver.realsense_wapper import Realsense
cam = Realsense(frame_width=1280, frame_height=720, fps=30)
while (True):
_, color_frame = cam.get_frame_cv()
corners, ids = detect_ar_marker(color_frame,
cam.get_intrinsics_matrix(),
cam.get_distortion_coeffs())
# print("Cornors:", corners)
# print("IDs： ", ids)
visualize_img = color_frame
if (len(corners) > 0):
R, t, _ = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, cam.get_intrinsics_matrix(),
cam.get_distortion_coeffs())
R_mat, _ = cv.Rodrigues(R)
# R_mat = np.squeeze(R_mat, 0)
t = np.squeeze(t, 1).transpose()
arm_target_yaw = round(yaw + rotation_sample[2], 5)
print("move target: ", [
arm_target_x, arm_target_y, arm_target_z, arm_target_roll,
arm_target_pitch, arm_target_yaw
])
arm.move_p([
arm_target_x, arm_target_y, arm_target_z, arm_target_roll,
arm_target_pitch, arm_target_yaw
])
_, color_frame = cam.get_frame_cv()
cam_T_marker_R, cam_T_marker_t, visualize_img = ar_marker.get_mat_cam_T_marker(
color_frame, maker_size, cam.get_intrinsics_matrix(),
cam.get_distortion_coeffs(), sample_repeat_num)
base_T_EE_R, base_T_EE_t = arm.getMatrixO_T_EE()
# print("End Effector in base frame:\n", "R:\n", base_T_EE_R, "t:\n", base_T_EE_t)
# print("End Effector in base frame:\n", arm_base_pt)
cv2.imshow("visualize_img", visualize_img)
cv2.waitKey(5)
if len(cam_T_marker_R) != 0:
EE_T_base_R = inv(base_T_EE_R)
EE_T_base_t = -inv(base_T_EE_R).dot(base_T_EE_t)
cam_T_marker_mats_R.append(cam_T_marker_R)
cam_T_marker_mats_t.append(cam_T_marker_t)