loop

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)

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]