conv visualize

robot/conveyor.py

@@ -0,0 +1,119 @@
+import pybullet as p
+import time
+import threading
+
+
+class ConveyorBelt:
+    def __init__(self, size, position, box_size, box_spacing, box_speed):
+        self.conveyor_size = size
+        self.conveyor_position = position
+        self.conveyor_id = self.create_conveyor(size, position)
+        self.box_size = box_size
+        self.box_spacing = box_spacing
+        self.box_speed = box_speed
+        self.boxes = []
+        self.create_boxes()
+        self.stop_thread = False
+        self.thread = threading.Thread(target=self.run)
+        self.thread.start()
+
+    def create_conveyor(self, size, position):
+        collision_shape_id = p.createCollisionShape(
+            shapeType=p.GEOM_BOX,
+            halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0],
+        )
+        visual_shape_id = p.createVisualShape(
+            shapeType=p.GEOM_BOX,
+            halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0],
+            rgbaColor=[0.5, 0.5, 0.5, 1],
+        )
+        conveyor_id = p.createMultiBody(
+            baseMass=0.0,
+            baseCollisionShapeIndex=collision_shape_id,
+            baseVisualShapeIndex=visual_shape_id,
+            basePosition=position,
+        )
+        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)
+        )
+        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(self.conveyor_size[0] / (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))
+
+    def create_box(self, position):
+        collision_shape_id = p.createCollisionShape(
+            shapeType=p.GEOM_BOX,
+            halfExtents=[
+                self.box_size[0] / 2.0,
+                self.box_size[1] / 2.0,
+                self.box_size[2] / 2.0,
+            ],
+        )
+        visual_shape_id = p.createVisualShape(
+            shapeType=p.GEOM_BOX,
+            halfExtents=[
+                self.box_size[0] / 2.0,
+                self.box_size[1] / 2.0,
+                self.box_size[2] / 2.0,
+            ],
+            rgbaColor=[1, 0, 0, 1],
+        )
+        box_id = p.createMultiBody(
+            baseMass=1.0,
+            baseCollisionShapeIndex=collision_shape_id,
+            baseVisualShapeIndex=visual_shape_id,
+            basePosition=position,
+            baseOrientation=p.getQuaternionFromEuler([0, 0, 0]),
+        )
+        return box_id
+
+    def move_boxes(self):
+        # Двигаем коробки вдоль конвейера
+        for box_id in self.boxes[:]:
+            # Получаем текущую позицию коробки
+            pos, orn = p.getBasePositionAndOrientation(box_id)
+
+            # Проверяем, если коробка выходит за пределы конвейера по оси X
+            # Начальное положение коробки + её длина (по оси X)
+            if (
+                pos[0] + self.box_size[0] * 0.5
+                > self.conveyor_position[0] + self.conveyor_size[0] * 0.5
+            ):
+                # Если коробка выходит за конец конвейера, она падает вниз (физика продолжает действовать)
+                # Удаляем коробку с конвейера
+                self.boxes.remove(box_id)
+            else:
+                # Двигаем коробку вдоль конвейера (по оси X)
+                new_pos = [pos[0] + self.box_speed, pos[1], pos[2]]
+                p.resetBasePositionAndOrientation(box_id, new_pos, orn)
+
+        # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество
+        if len(self.boxes) < int(
+            self.conveyor_size[0] / (self.box_size[0] + self.box_spacing)
+        ):
+            new_box = self.create_box(self.box_position())
+            self.boxes.append(new_box)
+
+    def run(self):
+        while not self.stop_thread:
+            self.move_boxes()
+            time.sleep(1)
+
+    def stop(self):
+        self.stop_thread = True
+        self.thread.join()

robot/urdf_manager.py

@@ -1,19 +1,13 @@
-import socket
-import json
 import os
-import math
 import numpy as np
 import time
 
-from typing import Literal
-from pprint import pprint
-
-import threading
 import pybullet as p
-import pybullet_industrial as pi
 
 from logger import logger
 
+from robot.conveyor import ConveyorBelt
+
 
 class UrdfManager:
     urdf_filename = None
@@ -35,65 +29,21 @@ class UrdfManager:
         self.physics_client = p.connect(getattr(p, type))
         logger.info(f"Connect to {self.physics_client} by {type}")
 
-    def create_cube(self, size, position):
-        # Создание формы коллизии для куба
-        collision_shape_id = p.createCollisionShape(
-            shapeType=p.GEOM_BOX,
-            halfExtents=[
-                size[0] / 2.0,
-                size[1] / 2.0,
-                size[2] / 2.0,
-            ],  # halfExtents задаются радиусами по осям x, y и z
-        )
-
-        # Создание визуальной формы для куба
-        visual_shape_id = p.createVisualShape(
-            shapeType=p.GEOM_BOX,
-            halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0],
-            rgbaColor=[0, 0, 1, 1],  # Синий цвет
-        )
-
-        # Создание многосоставного тела с формой коллизии и визуальной формой
-        body_id = p.createMultiBody(
-            baseMass=1.0,  # Масса тела
-            baseCollisionShapeIndex=collision_shape_id,
-            baseVisualShapeIndex=visual_shape_id,
-            basePosition=position,
-        )
-
-        return body_id
-
-    def create_conveyor(self, size, position):
-        collision_shape_id = p.createCollisionShape(
-            shapeType=p.GEOM_BOX,
-            halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0],
-        )
-        # Создание визуальной формы для конвейера
-        visual_shape_id = p.createVisualShape(
-            shapeType=p.GEOM_BOX,
-            halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0],
-            rgbaColor=[0.5, 0.5, 0.5, 1],  # Серый цвет для конвейера
-        )
-
-        # Создание тела для конвейера (можно без массы, если оно не должно взаимодействовать с другими объектами)
-        conveyor_id = p.createMultiBody(
-            baseMass=0.0,  # Это статичный объект, поэтому масса 0
-            baseCollisionShapeIndex=collision_shape_id,
-            baseVisualShapeIndex=visual_shape_id,
-            basePosition=position,
-        )
-
-        return conveyor_id
-
     def load_models(self):
-        # p.loadURDF("urdf/plane.urdf", physicsClientId=self.physics_client)
+        p.loadURDF(
+            os.path.join(self.application_path, "urdf", "plane.urdf"),
+            physicsClientId=self.physics_client,
+        )
 
         urdf_path = os.path.join(
             self.application_path, "urdf", f"{self.urdf_filename}.urdf"
         )
         self.body_id = p.loadURDF(
-            urdf_path, self.robot_start_position, useFixedBase=True
+            urdf_path,
+            self.robot_start_position,
+            useFixedBase=True
         )
+        p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
 
     def load_palletizing(self):
         # Путь к URDF модели палеты
@@ -116,42 +66,17 @@ class UrdfManager:
             / 2.0,  # Разместите палету выше, чтобы она не находилась на земле
         ]
         pallet_orientation = p.getQuaternionFromEuler([0, 0, np.deg2rad(90)])
-        self.pallet_id = p.loadURDF(
+        # self.pallet_id = p.loadURDF(
-            urdf_path_pallet, pallet_position, pallet_orientation, useFixedBase=True
+        #     urdf_path_pallet, pallet_position, pallet_orientation, useFixedBase=True
-        )
+        # )
 
-        # Создание коробок на палете
+        conveyor_size = [3.0, 0.5, 0.1]
-        box_size = [0.2, 0.2, 0.2]  # Размеры коробки: [ширина, высота, глубина]
+        conveyor_position = [0.5, 1.5, 0.5]
-        boxes_per_row = 3
+        box_size = [0.2, 0.2, 0.2]
-        boxes_per_column = 2
+        box_spacing = 0.2
+        box_speed = 0.1  # Скорость движения коробок
 
-        for row in range(boxes_per_column):
+        ConveyorBelt(conveyor_size, conveyor_position, box_size, box_spacing, box_speed)
-            for col in range(boxes_per_row):
-                box_x_offset = col * (box_size[0] + 0.05)
-                box_y_offset = row * (box_size[1] + 0.05)
-                box_z_position = pallet_height + (box_size[2] / 2)
-
-                box_position = [
-                    pallet_position[0]
-                    + box_x_offset
-                    - (boxes_per_row * (box_size[0] + 0.05) / 2),
-                    pallet_position[1]
-                    + box_y_offset
-                    - (boxes_per_column * (box_size[1] + 0.05) / 2),
-                    box_z_position,
-                ]
-
-                # Создаем кубик
-                self.create_cube(box_size, box_position)
-
-        # Создание конвейера
-        conveyor_size = [3.0, 0.5, 0.1]  # Длина, ширина и высота
-        conveyor_position = [
-            0.5,
-            1.5,
-            0.5,
-        ]  # Позиция в пространстве (выше уровня земли)
-        self.conveyor_id = self.create_conveyor(conveyor_size, conveyor_position)
 
         time.sleep(1)