0, 0
] # pose: perpendicular to the plane
arm.move_p(prepare_pos)
cs.open()
arm.move_p([
target_in_base[0], target_in_base[1],
target_in_base[2] + effector_offset, 3.14, 0, 0
])
cs.close()
time.sleep(0.5)
''' Move to check position '''
# arm.move_p(check_position)
arm.move_p(initial_pose)
''' Perform success check '''
_, color_check = cam.get_frame_cv()
bbox_check = get_obj_bbox(color_check, y1, y2, x1, x2)
data = []
print("current area = %f" % (bbox_check[2] * bbox_check[3]))
print("threshold area = %f" % check_threshold)
if (bbox_check[2] * bbox_check[3] < check_threshold):
print("Grasping SUCCESS in attempt {}".format(current_num))
data.append(str(current_num))
data.append(str(1)) # Success
else:
print("Grasping FAIL in attempt {}".format(current_num))
data.append(str(current_num))
data.append(str(0)) # Fail
# cv2.imshow('binary', binary)
# cv2.imshow('bbox', img)
# cv2.waitKey(10)
return x, y, w, h
if __name__ == '__main__':
""" Initialization """
# camera and robot driver
print('work_dir: ', _root_path)
cam = Realsense(frame_width=1280, frame_height=720, fps=30, exposure=800, white_balance=3500, contrast=80, sharpness=80)
_, img = cam.get_frame_cv()
y1 = 269
y2 = 542
x1 = 475
x2 = 799
crop = img[y1:y2, x1:x2]
gray = cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY)
blur = cv2.blur(gray, (4, 4))
_, binary = cv2.threshold(blur, 130, 256, cv2.THRESH_BINARY)
x, y, w, h = cv2.boundingRect(binary)
x += x1
# print("Found marker, H = ", "\n", H)
return R, t, visualize_img
# 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())
print('work_dir: ', _root_path)
import cv2
# from deepclaw.driver.sensors.camera.Realsense_L515 import Realsense
from Grasping_Franka.Driver.realsense_wapper import Realsense
if __name__ == '__main__':
# camera and robot driver
print('work_dir: ', _root_path)
'''
camera = Realsense('./Grasping_Franka/config/camera_rs_d435.yaml')
frame = camera.get_frame()
color = frame.color_image[0]
depth_img = frame.depth_image[0]
'''
camera = Realsense(frame_width=1280, frame_height=720, fps=30)
depth, color = camera.get_frame_cv()
cv2.imshow("img", color)
cv2.waitKey(1000)
crop = color[178:542, 433:891]
cv2.imshow("crop", crop)
cv2.waitKey(10000)
cv2.destroyAllWindows()