From b82ebbd9fb2a08a4d91449d671e034d6b0cded17 Mon Sep 17 00:00:00 2001
From: Kseninia Mikhaylova <k.mikhailova@svs-tech.pro>
Date: Fri, 13 Dec 2024 16:27:27 +0300
Subject: [PATCH] pallet

---
 robot/conveyor.py     | 16 ++++------------
 robot/pallet.py       | 37 +++++++++++++++++++++++++++++++++++++
 robot/urdf_manager.py |  7 ++-----
 3 files changed, 43 insertions(+), 17 deletions(-)
 create mode 100644 robot/pallet.py

diff --git a/robot/conveyor.py b/robot/conveyor.py
index bd5696c..0ee209b 100644
--- a/robot/conveyor.py
+++ b/robot/conveyor.py
@@ -39,11 +39,11 @@ class ConveyorBelt:
     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],
+            halfExtents=[size[i] / 2.0 for i in range(3)],
         )
         visual_shape_id = p.createVisualShape(
             shapeType=p.GEOM_BOX,
-            halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0],
+            halfExtents=[size[i] / 2.0 for i in range(3)],
             rgbaColor=[0.5, 0.5, 0.5, 1],
         )
         conveyor_id = p.createMultiBody(
@@ -75,19 +75,11 @@ class ConveyorBelt:
     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,
-            ],
+            halfExtents=[self.box_size[i] / 2.0 for i in range(3)],
         )
         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,
-            ],
+            halfExtents=[self.box_size[i] / 2.0 for i in range(3)],
             rgbaColor=[1, 0, 0, 1],
         )
         box_id = p.createMultiBody(
diff --git a/robot/pallet.py b/robot/pallet.py
new file mode 100644
index 0000000..1741eb8
--- /dev/null
+++ b/robot/pallet.py
@@ -0,0 +1,37 @@
+import pybullet as p
+
+
+class Pallet:
+    def __init__(
+        self, target=[1, 1, 0], size=[0.8, 1.2, 0.144], color=[0.6, 0.3, 0.1, 0.5]
+    ):
+        self.size = size
+        self.position = [
+            (target[i] - self.size[i] * 0.5)
+            for i in range(len(target))
+        ]
+        self.color = color
+
+        self.create_pallet()
+
+    def create_pallet(self):
+        half_size = [s * 0.5 for s in self.size]
+
+        # Создаем collision shape для паллеты
+        collision_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=half_size)
+
+        # Создаем визуальное отображение (shape) для паллеты
+        visual_shape = p.createVisualShape(
+            p.GEOM_BOX, halfExtents=half_size, rgbaColor=self.color
+        )
+
+        # Создаем объект паллеты в физическом мире
+        self.pallet_id = p.createMultiBody(
+            baseMass=0,  # Масса 0, чтобы паллета не взаимодействовала с физикой
+            baseCollisionShapeIndex=collision_shape,
+            baseVisualShapeIndex=visual_shape,
+            basePosition=self.position,
+        )
+
+        # Устанавливаем позицию объекта паллеты в физическом мире
+        p.resetBasePositionAndOrientation(self.pallet_id, self.position, [0, 0, 0, 1])
diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py
index 692bff5..6378677 100644
--- a/robot/urdf_manager.py
+++ b/robot/urdf_manager.py
@@ -7,6 +7,7 @@ import pybullet as p
 from logger import logger
 
 from robot.conveyor import ConveyorBelt
+from robot.pallet import Pallet
 
 
 class UrdfManager:
@@ -48,11 +49,7 @@ class UrdfManager:
 
     def load_palletizing(self):
         # p.resetSimulation()
-        pallet_position = [1.5, -1.5, 0.1]
-        urdf_path_pallet = os.path.join(
-            self.application_path, "urdf", "europallet.urdf"
-        )
-        self.pallet_id = p.loadURDF(urdf_path_pallet, pallet_position)
+        self.pallet_id = Pallet([1.5, 0.5, 0.1])
 
         # положение дальнего угла
         self.conv_target = [1.35, -0.75, 0.5]