Compare commits

..

3 Commits

Author SHA1 Message Date
Kseninia Mikhaylova fbd148ed31 pick 2024-12-13 16:00:40 +03:00
Kseninia Mikhaylova dacbe2ec74 wall pos 2024-12-13 15:51:55 +03:00
Kseninia Mikhaylova 72a5eb2bbd conv pos 2024-12-13 15:43:53 +03:00
5 changed files with 69 additions and 48 deletions

View File

@ -5,9 +5,10 @@ from logger import logger
class Palletizing(QWidget): class Palletizing(QWidget):
def __init__(self, load_palletizing): def __init__(self, load_palletizing, start_moving):
super().__init__() super().__init__()
self.load_palletizing = load_palletizing self.load_palletizing = load_palletizing
self.start_moving = start_moving
self.initUI() self.initUI()
def initUI(self): def initUI(self):
@ -19,12 +20,18 @@ class Palletizing(QWidget):
self.palletButton = QPushButton("Визуализировать") self.palletButton = QPushButton("Визуализировать")
self.palletButton.clicked.connect(self.startPalletzing) self.palletButton.clicked.connect(self.startPalletzing)
self.layout.addWidget(self.palletButton) self.layout.addWidget(self.palletButton)
self.startButton = QPushButton("Конвейер")
self.startButton.clicked.connect(self.startMoving)
self.layout.addWidget(self.startButton)
self.setLayout(self.layout) self.setLayout(self.layout)
def startPalletzing(self): def startPalletzing(self):
logger.info("start palletizing")
self.load_palletizing() self.load_palletizing()
def startMoving(self):
self.start_moving()
def paintEvent(self, event): def paintEvent(self, event):
p = self.palette() p = self.palette()

View File

@ -73,6 +73,7 @@ class MyApp:
}, },
palletizingPanel={ palletizingPanel={
"load_palletizing": self.load_palletizing, "load_palletizing": self.load_palletizing,
"start_moving": self.start_moving
}, },
) )
mainWindow.setWindowTitle("ROBOT GUI") mainWindow.setWindowTitle("ROBOT GUI")
@ -181,6 +182,10 @@ class MyApp:
@check_robot_app @check_robot_app
def load_palletizing(self): def load_palletizing(self):
return self.robot_app.load_palletizing() return self.robot_app.load_palletizing()
@check_robot_app
def start_moving(self):
return self.robot_app.start_moving()
if __name__ == "__main__": if __name__ == "__main__":

View File

@ -78,6 +78,9 @@ class SocketRobotArm:
def load_palletizing(self): def load_palletizing(self):
self.urdf_manager.load_palletizing() self.urdf_manager.load_palletizing()
def start_moving(self):
self.urdf_manager.conveyor.moving = True
def get_pybullet_image(self): def get_pybullet_image(self):
return self.urdf_manager.get_pybullet_image() return self.urdf_manager.get_pybullet_image()
@ -201,19 +204,19 @@ class SocketRobotArm:
threading.Thread(target=self.imitate_func, daemon=True).start() threading.Thread(target=self.imitate_func, daemon=True).start()
def palletizing(self, position, box_id): def palletizing(self, position, box_id):
pos = position[:] dir_wall = (self.urdf_manager.conv_direction + 1) % 2
conv = position[:]
conv[1] += 0.4
position[1] += 0.02 conv = position[:]
conv[dir_wall] -= 0.4
position[dir_wall] -= 0.02
json_res = [] json_res = []
tool_pos = [0, 0, 0]
for pos in [conv, position]: for pos in [conv, position]:
jointPoses = self.convert_to_joint_base( jointPoses = self.convert_to_joint_base(
pos, pos,
# np.radians([0, 0, 0]), np.radians(tool_pos),
np.radians([0, 0, -90]),
) )
json_res.append( json_res.append(
self.prepare_data.make_step( self.prepare_data.make_step(
@ -222,23 +225,10 @@ class SocketRobotArm:
) )
self.motionFund(jointPoses) self.motionFund(jointPoses)
time.sleep(2) time.sleep(2)
with open(f"data/palletizing.json", "w") as myfile: with open(f"data/palletizing.json", "w") as myfile:
myfile.write(json.dumps(json_res)) myfile.write(json.dumps(json_res))
end_effector_state = p.getLinkState(
self.body_id, self.num_joints - 1, computeForwardKinematics=True
)
end_effector_pos = end_effector_state[0] # Позиция наконечника
end_effector_orn = end_effector_state[1] # Ориентация наконечника
object_pos, object_orn = p.getBasePositionAndOrientation(box_id)
# Используем ориентацию объекта (коробки) как ориентацию родителя
parentFrameOrientation = object_orn
# # Определяем смещение для фиксации позиции (без вращения)
parentFramePosition = [pos[i] - end_effector_pos[i] for i in range(3)]
# Создаём констрейнт для прикрепления # Создаём констрейнт для прикрепления
constraint_id = p.createConstraint( constraint_id = p.createConstraint(
parentBodyUniqueId=self.body_id, parentBodyUniqueId=self.body_id,
@ -247,7 +237,7 @@ class SocketRobotArm:
childLinkIndex=-1, childLinkIndex=-1,
jointType=p.JOINT_FIXED, jointType=p.JOINT_FIXED,
jointAxis=[0, 0, 0], jointAxis=[0, 0, 0],
parentFramePosition=[0,0,0], parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0.2, 0], childFramePosition=[0, 0.2, 0],
parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 90])), parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 90])),
childFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])), childFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])),

View File

@ -5,9 +5,20 @@ from logger import logger
class ConveyorBelt: class ConveyorBelt:
moving = False
def __init__( def __init__(
self, size, position, box_size, box_spacing, box_speed, palletizinFunc self,
size,
position,
direction,
box_size,
box_spacing,
box_speed,
palletizinFunc,
): ):
self.direction = direction
self.conveyor_size = size self.conveyor_size = size
self.conveyor_position = position self.conveyor_position = position
self.conveyor_id = self.create_conveyor(size, position) self.conveyor_id = self.create_conveyor(size, position)
@ -44,21 +55,19 @@ class ConveyorBelt:
return conveyor_id return conveyor_id
def box_position(self, i=0): def box_position(self, i=0):
x_position = ( position = self.conveyor_position[:]
self.conveyor_position[0] position[self.direction] = (
- self.conveyor_size[0] * 0.5 self.conveyor_position[self.direction]
+ i * (self.box_size[0] + self.box_spacing) - self.conveyor_size[self.direction] * 0.5
+ i * (self.box_size[self.direction] + self.box_spacing)
+ self.box_size[0] * 0.5
) )
y_position = ( return position
self.conveyor_position[1] + self.conveyor_size[1] * 0.5 - self.box_size[1]
)
z_position = (
self.conveyor_position[2] + self.conveyor_size[2] + self.box_size[2] * 0.5
)
return [x_position, y_position, z_position]
def create_boxes(self): def create_boxes(self):
num_boxes = int(self.conveyor_size[0] / (self.box_size[0] + self.box_spacing)) num_boxes = int(
self.conveyor_size[self.direction] / (self.box_size[0] + self.box_spacing)
)
for i in range(num_boxes): for i in range(num_boxes):
position = self.box_position(i) position = self.box_position(i)
self.boxes.append(self.create_box(position)) self.boxes.append(self.create_box(position))
@ -91,23 +100,26 @@ class ConveyorBelt:
return box_id return box_id
def move_boxes(self): def move_boxes(self):
if not self.conveyor_stopped: if self.moving and not self.conveyor_stopped:
for box_id in self.boxes[:]: for box_id in self.boxes[:]:
pos, orn = p.getBasePositionAndOrientation(box_id) pos, orn = p.getBasePositionAndOrientation(box_id)
if ( if (
pos[0] + self.box_size[0] * 0.5 pos[self.direction] + self.box_size[0] * 0.5
> self.conveyor_position[0] + self.conveyor_size[0] * 0.5 > self.conveyor_position[self.direction]
+ self.conveyor_size[self.direction] * 0.5
): ):
# Если коробка выходит за конец конвейера, она падает вниз (физика продолжает действовать) # Если коробка выходит за конец конвейера, она падает вниз (физика продолжает действовать)
# Удаляем коробку с конвейера # Удаляем коробку с конвейера
self.boxes.remove(box_id) self.boxes.remove(box_id)
self.conveyor_stopped = True self.conveyor_stopped = True
right_wall_position = [pos[0], pos[1] + self.box_size[1] * 0.5, pos[2]] wall_position = list(pos)
self.call_robot(right_wall_position, box_id) dir_wall = (self.direction + 1) % (3 - 1)
wall_position[dir_wall] -= self.box_size[dir_wall] * 0.5
self.call_robot(wall_position, box_id)
else: else:
# Двигаем коробку вдоль конвейера (по оси X) new_pos = list(pos[:])
new_pos = [pos[0] + self.box_speed, pos[1], pos[2]] new_pos[self.direction] += self.box_speed
p.resetBasePositionAndOrientation(box_id, new_pos, orn) p.resetBasePositionAndOrientation(box_id, new_pos, orn)
# Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество

View File

@ -54,15 +54,22 @@ class UrdfManager:
) )
self.pallet_id = p.loadURDF(urdf_path_pallet, pallet_position) self.pallet_id = p.loadURDF(urdf_path_pallet, pallet_position)
self.conv_pos = [-1.5, -1.5, 0.5] # положение дальнего угла
self.conv_size = [3.0, 0.5, 0.1] self.conv_target = [1.35, -0.75, 0.5]
ConveyorBelt( self.conv_size = [0.5, 3.0, 0.1]
self.conv_direction = self.conv_size.index(max(self.conv_size))
self.conv_pos = [
(self.conv_target[i] - self.conv_size[i] * 0.5)
for i in range(len(self.conv_target))
]
self.conveyor = ConveyorBelt(
self.conv_size, self.conv_size,
self.conv_pos, self.conv_pos,
self.conv_direction,
[0.2, 0.2, 0.2], [0.2, 0.2, 0.2],
0.2, 0.2,
0.1, 0.1,
self.palletizing self.palletizing,
) )
time.sleep(1) time.sleep(1)