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

---
 robot/client_socket.py | 17 ++++++++++++++++-
 robot/urdf_manager.py  |  2 +-
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/robot/client_socket.py b/robot/client_socket.py
index b77a8b5..50b4050 100644
--- a/robot/client_socket.py
+++ b/robot/client_socket.py
@@ -224,7 +224,7 @@ class SocketRobotArm:
                 )
             )
             self.motionFund(jointPoses)
-            time.sleep(2)
+            # time.sleep(2)
 
         with open(f"data/palletizing.json", "w") as myfile:
             myfile.write(json.dumps(json_res))
@@ -242,6 +242,21 @@ class SocketRobotArm:
             parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 90])),
             childFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])),
         )
+        pal_pos = self.urdf_manager.pallet.position
+        for pos in [conv, pal_pos]:
+            jointPoses = self.convert_to_joint_base(
+                pos,
+                np.radians(tool_pos),
+            )
+            json_res.append(
+                self.prepare_data.make_step(
+                    "free", np.degrees(jointPoses), [0, 0, 0, 0, 0, 0]
+                )
+            )
+            self.motionFund(jointPoses)
+            # time.sleep(2)
+        p.removeConstraint(constraint_id)
+        self.urdf_manager.conveyor.conveyor_stopped = False
 
     def set_text(self, text):
         logger.info(text)
diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py
index 9aaaa18..3e447b7 100644
--- a/robot/urdf_manager.py
+++ b/robot/urdf_manager.py
@@ -49,7 +49,7 @@ class UrdfManager:
 
     def load_palletizing(self):
         # p.resetSimulation()
-        self.pallet_id = Pallet([1.3, 0.6, 0.1])
+        self.pallet = Pallet([1.3, 0.6, 0.1])
 
         # положение дальнего угла
         self.conv_target = [1.1, -0.63, 0.4]