pick

gui/palletizing.py

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

main.py

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

robot/client_socket.py

@@ -78,6 +78,9 @@ class SocketRobotArm:
     def load_palletizing(self):
         self.urdf_manager.load_palletizing()
 
+    def start_moving(self):
+        self.urdf_manager.conveyor.moving = True
+
     def get_pybullet_image(self):
         return self.urdf_manager.get_pybullet_image()
 
@@ -201,19 +204,19 @@ class SocketRobotArm:
         threading.Thread(target=self.imitate_func, daemon=True).start()
 
     def palletizing(self, position, box_id):
-        pos = position[:]
-        conv = position[:]
-        conv[1] += 0.4
+        dir_wall = (self.urdf_manager.conv_direction + 1) % 2
 
-        position[1] += 0.02
+        conv = position[:]
+        conv[dir_wall] -= 0.4
+        position[dir_wall] -= 0.02
 
         json_res = []
+        tool_pos = [0, 0, 0]
 
         for pos in [conv, position]:
             jointPoses = self.convert_to_joint_base(
                 pos,
-                # np.radians([0, 0, 0]),
-                np.radians([0, 0, -90]),
+                np.radians(tool_pos),
             )
             json_res.append(
                 self.prepare_data.make_step(
@@ -222,23 +225,10 @@ class SocketRobotArm:
             )
             self.motionFund(jointPoses)
             time.sleep(2)
+
         with open(f"data/palletizing.json", "w") as myfile:
             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(
             parentBodyUniqueId=self.body_id,
@@ -247,7 +237,7 @@ class SocketRobotArm:
             childLinkIndex=-1,
             jointType=p.JOINT_FIXED,
             jointAxis=[0, 0, 0],
-            parentFramePosition=[0,0,0],
+            parentFramePosition=[0, 0, 0],
             childFramePosition=[0, 0.2, 0],
             parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 90])),
             childFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])),

robot/conveyor.py

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

robot/urdf_manager.py

@@ -54,15 +54,22 @@ class UrdfManager:
         )
         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]
-        ConveyorBelt(
+        # положение дальнего угла
+        self.conv_target = [1.35, -0.75, 0.5]
+        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_pos,
+            self.conv_direction,
             [0.2, 0.2, 0.2],
             0.2,
             0.1,
-            self.palletizing
+            self.palletizing,
         )
 
         time.sleep(1)