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:
