gui/palletizing.py

@@ -5,10 +5,9 @@ from logger import logger
 
 
 class Palletizing(QWidget):
-    def __init__(self, load_palletizing, start_moving):
+    def __init__(self, load_palletizing):
         super().__init__()
         self.load_palletizing = load_palletizing
-        self.start_moving = start_moving
         self.initUI()
 
     def initUI(self):
@@ -20,18 +19,12 @@ 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,7 +73,6 @@ class MyApp:
             },
             palletizingPanel={
                 "load_palletizing": self.load_palletizing,
-                "start_moving": self.start_moving
             },
         )
         mainWindow.setWindowTitle("ROBOT GUI")
@@ -182,10 +181,6 @@ 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,9 +78,6 @@ 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()
 
@@ -204,19 +201,19 @@ class SocketRobotArm:
         threading.Thread(target=self.imitate_func, daemon=True).start()
 
     def palletizing(self, position, box_id):
-        dir_wall = (self.urdf_manager.conv_direction + 1) % 2
+        pos = position[:]
-
         conv = position[:]
-        conv[dir_wall] -= 0.4
+        conv[1] += 0.4
-        position[dir_wall] -= 0.02
+
+        position[1] += 0.02
 
         json_res = []
-        tool_pos = [0, 0, 0]
 
         for pos in [conv, position]:
             jointPoses = self.convert_to_joint_base(
                 pos,
-                np.radians(tool_pos),
+                # np.radians([0, 0, 0]),
+                np.radians([0, 0, -90]),
             )
             json_res.append(
                 self.prepare_data.make_step(
@@ -225,10 +222,23 @@ 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,
@@ -237,7 +247,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,20 +5,9 @@ from logger import logger
 
 
 class ConveyorBelt:
-    moving = False
-
     def __init__(
-        self,
+        self, size, position, box_size, box_spacing, box_speed, palletizinFunc
-        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)
@@ -55,19 +44,21 @@ class ConveyorBelt:
         return conveyor_id
 
     def box_position(self, i=0):
-        position = self.conveyor_position[:]
+        x_position = (
-        position[self.direction] = (
+            self.conveyor_position[0]
-            self.conveyor_position[self.direction]
+            - self.conveyor_size[0] * 0.5
-            - self.conveyor_size[self.direction] * 0.5
+            + i * (self.box_size[0] + self.box_spacing)
-            + i * (self.box_size[self.direction] + self.box_spacing)
-            + self.box_size[0] * 0.5
         )
-        return position
+        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]
 
     def create_boxes(self):
-        num_boxes = int(
+        num_boxes = int(self.conveyor_size[0] / (self.box_size[0] + self.box_spacing))
-            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))
@@ -100,26 +91,23 @@ class ConveyorBelt:
         return box_id
 
     def move_boxes(self):
-        if self.moving and not self.conveyor_stopped:
+        if not self.conveyor_stopped:
             for box_id in self.boxes[:]:
                 pos, orn = p.getBasePositionAndOrientation(box_id)
+
                 if (
-                    pos[self.direction] + self.box_size[0] * 0.5
+                    pos[0] + self.box_size[0] * 0.5
-                    > self.conveyor_position[self.direction]
+                    > self.conveyor_position[0] + self.conveyor_size[0] * 0.5
-                    + self.conveyor_size[self.direction] * 0.5
                 ):
                     # Если коробка выходит за конец конвейера, она падает вниз (физика продолжает действовать)
                     # Удаляем коробку с конвейера
                     self.boxes.remove(box_id)
                     self.conveyor_stopped = True
-                    wall_position = list(pos)
+                    right_wall_position = [pos[0], pos[1] + self.box_size[1] * 0.5, pos[2]]
-                    dir_wall = (self.direction + 1) % (3 - 1)
+                    self.call_robot(right_wall_position, box_id)
-                    wall_position[dir_wall] -= self.box_size[dir_wall] * 0.5
-                    
-                    self.call_robot(wall_position, box_id)
                 else:
-                    new_pos = list(pos[:])
+                    # Двигаем коробку вдоль конвейера (по оси X)
-                    new_pos[self.direction] += self.box_speed
+                    new_pos = [pos[0] + self.box_speed, pos[1], pos[2]]
                     p.resetBasePositionAndOrientation(box_id, new_pos, orn)
 
         # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество

robot/urdf_manager.py

@@ -54,22 +54,15 @@ class UrdfManager:
         )
         self.pallet_id = p.loadURDF(urdf_path_pallet, pallet_position)
 
-        # положение дальнего угла
+        self.conv_pos = [-1.5, -1.5, 0.5]
-        self.conv_target = [1.35, -0.75, 0.5]
+        self.conv_size = [3.0, 0.5, 0.1]
-        self.conv_size = [0.5, 3.0, 0.1]
+        ConveyorBelt(
-        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)