From 410f22e5ba50d43df9e8935c7a540de58019084f Mon Sep 17 00:00:00 2001
From: Kseninia Mikhaylova <k.mikhailova@svs-tech.pro>
Date: Mon, 16 Dec 2024 13:13:19 +0300
Subject: [PATCH] pallete

---
 packer/packer.py       |  2 +-
 robot/client_socket.py | 34 ++++++++++++++++++++++++++++------
 robot/urdf_manager.py  | 18 +++++++++++++++---
 3 files changed, 44 insertions(+), 10 deletions(-)

diff --git a/packer/packer.py b/packer/packer.py
index 73ab84e..f760394 100644
--- a/packer/packer.py
+++ b/packer/packer.py
@@ -9,7 +9,7 @@ class PalletPacker:
         self.pallet_height = pallet.get("y", 0.8)
         self.pallet_depth = pallet.get("z", 0.144)
 
-        gap = 0.2
+        gap = 0
         self.box_width = box.get("x", 0.2) + gap
         self.box_height = box.get("y", 0.2) + gap
         self.box_depth = box.get("z", 0.2)
diff --git a/robot/client_socket.py b/robot/client_socket.py
index fcdef81..28ecac4 100644
--- a/robot/client_socket.py
+++ b/robot/client_socket.py
@@ -216,10 +216,18 @@ class SocketRobotArm:
         pallet_position = self.urdf_manager.packing[
             counter % len(self.urdf_manager.packing)
         ]
-        logger.info(counter)
-        logger.info(pallet_position)
+        logger.info(f"pallete {pallet_position}")
+
+        base1_position = pallet_position[:]
+        base1_position[0] -= 0.2
+        base1_position[2] += 0.2
+        
         base_position = pallet_position[:]
-        base_position[2] += 1.0
+        base_position[0] -= 0.2
+        
+        drop_position = pallet_position[:]
+        drop_position[0] -= 0.4
+        drop_position[2] += 0.4
 
         json_res = []
         tool_pos = [0, 0, 0]
@@ -228,14 +236,19 @@ class SocketRobotArm:
             {"type": "move", "coordinates": near_position},
             {"type": "move", "coordinates": object_position},
             {"type": "create_constraint"},
+            {"type": "move", "coordinates": base1_position},
+            {"type": "move", "coordinates": base_position},
             {"type": "move", "coordinates": pallet_position},
             {"type": "remove_constraint"},
             {"type": "move", "coordinates": base_position},
+            {"type": "move", "coordinates": base1_position},
+            {"type": "move", "coordinates": drop_position},
         ]
         constraint_id = None
 
         for step in steps:
             if step["type"] == "move":
+                # logger.info(step["coordinates"])
                 jointPoses = self.convert_to_joint_base(
                     step["coordinates"],
                     np.radians(tool_pos),
@@ -246,7 +259,10 @@ class SocketRobotArm:
                     )
                 )
                 self.motionFund(jointPoses)
+                # time.sleep(2)
             if step["type"] == "create_constraint":
+                link_state = p.getLinkState(self.body_id, self.num_joints - 1)
+                orientation = link_state[1]
                 constraint_id = p.createConstraint(
                     parentBodyUniqueId=self.body_id,
                     parentLinkIndex=self.num_joints - 1,
@@ -259,12 +275,18 @@ class SocketRobotArm:
                     parentFrameOrientation=p.getQuaternionFromEuler(
                         np.radians([0, 0, 90])
                     ),
-                    childFrameOrientation=p.getQuaternionFromEuler(
-                        np.radians([0, 0, 0])
-                    ),
+                    childFrameOrientation=orientation,
                 )
             if step["type"] == "remove_constraint":
+                time.sleep(0.5)
                 p.removeConstraint(constraint_id)
+                # time.sleep(0.5)
+                
+                # p.resetBasePositionAndOrientation(
+                #     box_id,
+                #     pallet_position,
+                #     p.getQuaternionFromEuler(np.radians([0, 0, 0])),
+                # )
 
         with open(f"data/palletizing.json", "w") as myfile:
             myfile.write(json.dumps(json_res))
diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py
index 03fe4b9..f401294 100644
--- a/robot/urdf_manager.py
+++ b/robot/urdf_manager.py
@@ -68,7 +68,7 @@ class UrdfManager:
         self.pallet = Pallet(self.pallet_position)
 
         self.box_size = [0.2, 0.2, 0.2]
-        
+
         # положение дальнего угла
         self.conv_target = [1.1 + 0.5, -0.63, 0.4]
         self.conv_size = [0.5, 3.0, 0.1]
@@ -99,10 +99,22 @@ class UrdfManager:
                 "z": self.box_size[2],
             },
         ).pack()
-        logger.info(self.packing)
+        self.packing = sorted(self.packing, key=lambda x: (x[0], x[1]))
+
         for p in self.packing:
+            # logger.info(p)
+            
             for i in range(len(self.box_size)):
-                p[i] = (self.pallet_position[i] - self.pallet_size[i] * 0.5) + self.box_size[i] * 0.5 - p[i]
+                p[i] = (
+                    self.pallet_target[i]
+                    - p[i]
+                    - self.box_size[i] * 0.5
+                )
+            dir_wall = (self.conv_direction + 1) % (3 - 1)
+            p[dir_wall] -= self.box_size[dir_wall]
+            p[self.conv_direction] -= self.box_size[self.conv_direction] * 0.5
+            p[2] += self.pallet_size[2]
+        # logger.info(f"{self.packing[0]} {self.pallet_target}")
 
         time.sleep(1)