def pallet(): global pallet_y, pallet_x, speed, detail_size, between_details, CURRENT_COLOR, CURRENT_BLOCK, pallet_furthest_point if interface.enableAllDetails.isChecked(): speed = int(interface.comboSpeed.currentText()) robot.set_position(camera_position, speed) robot.await_motion(asking_interval=0.001) img = stream.read() # features = detect(img, get_coordinates(robot.get_position())) # new = SHIFT_LIST[2] - float(features[0][3]) for y in range(pallet_y): for x in range(pallet_x): state = pick_new_block() if state == 'Continue': next_x = pallet_furthest_point[0] - x * \ (detail_size + between_details) next_y = pallet_furthest_point[1] + y * \ (detail_size + between_details) new = SHIFT_LIST[2] - float(features[0][3]) robot.set_position( position([next_x, next_y, 0.15], [SHIFT_LIST[0], SHIFT_LIST[1], new]), speed) robot.await_motion(asking_interval=0.001) robot.set_position( position([next_x, next_y, 0.04], [SHIFT_LIST[0], SHIFT_LIST[1], new]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) robot.close_gripper(1) # robot.set_position(position([next_x, next_y, 0.15], # [SHIFT_LIST[0], # SHIFT_LIST[1], # new]), # speed, # motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) CURRENT_BLOCK += 1 else: reply = QMessageBox.question( interface, 'Внимание', "Робот разложил {} кубиков, не обнаружил других " "и возвращается в начальное положение.".format( CURRENT_BLOCK), QMessageBox.Ok, QMessageBox.Ok) if reply == QMessageBox.Ok: stream.stop() robot.set_pose(pose([0, -90, 0, -90, -90, 0]), 10) return None set_camera_position() reply = QMessageBox.question( interface, 'Внимание', "Робот разложил все {} кубиков " "и возвращается в начальное положение.".format(CURRENT_BLOCK), QMessageBox.Ok | QMessageBox.Cancel, QMessageBox.Cancel) if reply == QMessageBox.Ok: stream.stop() robot.set_pose(pose([0, -90, 0, -90, -90, 0]), 10) return None
def pick_new_block(): global speed, img, features if interface.enableColorPalletising.isChecked( ) or not interface.enableAllDetails.isChecked(): speed = int(interface.comboSpeed.currentText()) robot.set_position(camera_position, speed) robot.await_motion(asking_interval=0.001) img = stream.read() features = detect(img, get_coordinates(robot.get_position())) if type(features) != str: # robot.set_position(position([features[0][0], features[0][1], 0.35], # [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), # speed) robot.await_motion(asking_interval=0.001) robot.set_position( position([features[0][0], features[0][1], 0.04], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) robot.open_gripper(1) robot.set_position( position([features[0][0], features[0][1], 0.15], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) return 'Continue' else: return None elif not interface.enableColorPalletising.isChecked( ) or interface.enableAllDetails.isChecked(): if type(features) != str: robot.set_position( position([ features[CURRENT_BLOCK][0], features[CURRENT_BLOCK][1], 0.15 ], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), speed) robot.await_motion(asking_interval=0.001) robot.set_position(position( [features[CURRENT_BLOCK][0], features[CURRENT_BLOCK][1], 0.04], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) robot.open_gripper(1) robot.set_position(position( [features[CURRENT_BLOCK][0], features[CURRENT_BLOCK][1], 0.15], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) return 'Continue' else: return None
def define_camera_place(): interface.furthest_corner = [ float(interface.entry_camera_x2.text()), float(interface.entry_camera_y2.text()), float(interface.entry_camera_z2.text()) ] interface.nearest_corner = [ float(interface.entry_camera_x1.text()), float(interface.entry_camera_y1.text()), float(interface.entry_camera_z1.text()) ] camera_x_coordinate = (interface.furthest_corner[0] + interface.nearest_corner[0]) / 2 + 0.035 camera_y_coordinate = (interface.furthest_corner[1] + interface.nearest_corner[1]) / 2 + 0.065 camera_coordinates = position( [camera_x_coordinate, camera_y_coordinate, 0.45], [math.pi, 0, -math.pi / 4]) return camera_coordinates
def keyboardControl(key): try: current_position = robot.get_position() xyz_list = get_coordinates(current_position) if key == 'W': robot.set_position( position([xyz_list[0], xyz_list[1] - 0.05, xyz_list[2]], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), 10) robot.await_motion(asking_interval=0.001) elif key == 'A': robot.set_position( position([xyz_list[0] + 0.05, xyz_list[1], xyz_list[2]], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), 10) robot.await_motion(asking_interval=0.001) elif key == 'S': robot.set_position( position([xyz_list[0], xyz_list[1] + 0.05, xyz_list[2]], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), 10) robot.await_motion(asking_interval=0.001) elif key == 'D': robot.set_position( position([xyz_list[0] - 0.05, xyz_list[1], xyz_list[2]], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), 10) robot.await_motion(asking_interval=0.001) elif key == 'Space': robot.set_position( position([xyz_list[0], xyz_list[1], xyz_list[2] + 0.01], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), 10) robot.await_motion(asking_interval=0.001) elif key == 'C': robot.set_position( position([xyz_list[0], xyz_list[1], xyz_list[2] - 0.01], [SHIFT_LIST[0], SHIFT_LIST[1], SHIFT_LIST[2]]), 10) robot.await_motion(asking_interval=0.001) elif key == 'Q': robot.open_gripper(1) elif key == 'E': robot.close_gripper(1) # print(get_coordinates(current_position)) except PulseApiException as e: QMessageBox.warning( interface, 'Внимание!', "Робот не может двигаться в дальнейшем направлении!.", QMessageBox.Ok) if robot.status_motion() == MotionStatus.ERROR: robot.recover() print('Robot recovered from error')
def create_tool(radius, height, name): """ The tool is created as cylinder with R = radius and H = height Important step in work process. With it help robot calculate trajectory considering his gripper/tool :param radius: cylinder radius :param height: cylinder height """ if 0 <= radius <= 0.3 and 0 <= height <= 0.3: # create new tool properties new_tool_info = tool_info(position([0, 0, height], [0, 0, 0]), name=name) new_tool_shape = tool_shape([ create_simple_capsule_obstacle(radius, Point(0, 0, 0), Point(0, 0, height)) ]) # change tool properties robot.change_tool_info(new_tool_info) robot.change_tool_shape(new_tool_shape) else: raise AssertionError('Incorrect tool parameters')
def color_pallet(): global pallet_y, pallet_x, speed, detail_size, between_details, CURRENT_COLOR, CURRENT_BLOCK, pallet_furthest_point state = pick_new_block() if state == 'Continue': CURRENT_COLOR = str(features[0][4]) if interface.enableAutoColors.isChecked( ) and CURRENT_COLOR not in interface.PALLET_LIST: new = SHIFT_LIST[2] - float(features[0][3]) # создание пары "новый цвет-пустой список" interface.PALLET_LIST.update({CURRENT_COLOR: []}) interface.PALLET_LIST[CURRENT_COLOR].append( len(interface.PALLET_LIST[CURRENT_COLOR])) CURRENT_PALLET_X = len(interface.PALLET_LIST[CURRENT_COLOR]) - 1 CURRENT_PALLET_Y = list( interface.PALLET_LIST.keys()).index(CURRENT_COLOR) next_x = pallet_furthest_point[0] - CURRENT_PALLET_X * ( detail_size + between_details) next_y = pallet_furthest_point[1] + CURRENT_PALLET_Y * ( detail_size + between_details) robot.set_position( position([next_x, next_y, 0.15], [SHIFT_LIST[0], SHIFT_LIST[1], new]), speed) robot.await_motion(asking_interval=0.001) robot.set_position(position([next_x, next_y, 0.04], [SHIFT_LIST[0], SHIFT_LIST[1], new]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) robot.close_gripper(1) robot.set_position(position([next_x, next_y, 0.06], [SHIFT_LIST[0], SHIFT_LIST[1], new]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) CURRENT_BLOCK += 1 color_pallet() else: new = SHIFT_LIST[2] - float(features[0][3]) interface.PALLET_LIST[CURRENT_COLOR].append( len(interface.PALLET_LIST[CURRENT_COLOR])) CURRENT_PALLET_X = len(interface.PALLET_LIST[CURRENT_COLOR]) - 1 CURRENT_PALLET_Y = list( interface.PALLET_LIST.keys()).index(CURRENT_COLOR) next_x = pallet_furthest_point[0] - CURRENT_PALLET_X * ( detail_size + between_details) next_y = pallet_furthest_point[1] + CURRENT_PALLET_Y * ( detail_size + between_details) robot.set_position( position([next_x, next_y, 0.15], [SHIFT_LIST[0], SHIFT_LIST[1], new]), speed) robot.await_motion(asking_interval=0.001) robot.set_position(position([next_x, next_y, 0.04], [SHIFT_LIST[0], SHIFT_LIST[1], new]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) robot.close_gripper(1) robot.set_position(position([next_x, next_y, 0.06], [SHIFT_LIST[0], SHIFT_LIST[1], new]), speed, motion_type=MT_LINEAR) robot.await_motion(asking_interval=0.001) CURRENT_BLOCK += 1 color_pallet() else: reply = QMessageBox.question( interface, 'Внимание', "Робот разложил {} кубиков и не обнаружил других." "Вернуться в начальное положение или попробовать обнаружить ещё?". format(CURRENT_BLOCK), QMessageBox.Ok | QMessageBox.Retry, QMessageBox.Retry) if reply == QMessageBox.Ok: stream.stop() robot.set_pose(pose([0, -90, 0, -90, -90, 0]), 10) return None elif reply == QMessageBox.Retry: color_pallet() return None
import cv2 import time # Настройка робота robot = RobotPulse('10.10.10.20:8081') # Вызов UI app = QApplication(sys.argv) interface = UI(app) interface.show() # Инициализация и подгрузка нейронной сети, задание смещения камеры nn_model = Model() KMeans = K_Means() stream = VideoStream(0) robot.change_base(position([0, 0, 0], [0, 0, 0])) roll, pitch = -0.016667, 0.012 robot.change_base(position([0, 0, -0.132], [-roll, -pitch, 0])) src = 0 def create_tool(radius, height, name): """ The tool is created as cylinder with R = radius and H = height Important step in work process. With it help robot calculate trajectory considering his gripper/tool :param radius: cylinder radius :param height: cylinder height """ if 0 <= radius <= 0.3 and 0 <= height <= 0.3: