export coord to robot

gui/command.py

@@ -35,7 +35,12 @@ class Command(QWidget):
         self.layout.addWidget(self.updButton)
 
         self.chgButton = QPushButton("Изменить тип")
-        self.chgButton.clicked.connect(lambda: self.changeType('calc' if self.command_type() == 'base' else 'base'))
+        cur_types = ["base", "calc", "pallette"]
+        self.chgButton.clicked.connect(
+            lambda: self.changeType(
+                cur_types[(cur_types.index(self.command_type()) + 1) % len(cur_types)]
+            )
+        )
         self.layout.addWidget(self.chgButton)
 
     def paintEvent(self, event):

gui/init.py

@@ -68,8 +68,8 @@ class MainContentComponent(QWidget):
                 self.informerPanel,
                 # self.filenamePanel,
                 # self.imitatorPanel,
-                # self.commandPanel,
                 self.palletizingPanel,
+                self.commandPanel,
             ],
             color=(169, 169, 169),
         )

robot/client_socket.py

@@ -173,11 +173,12 @@ class SocketRobotArm:
         self.set_text(text=f"Старт одиночного цикла")
         time.sleep(0.5)
 
-        commands = (
-            self.steps_from_file()
-            if self.command_type == "base"
-            else self.convert_point_to_free()
-        )
+        if self.command_type == "base":
+            commands = self.steps_from_file()
+        if self.command_type == "calc":
+            commands = self.convert_point_to_free()
+        if self.command_type == "pallette":
+            commands = self.get_palletizing_json()
         with open(f"log/{self.command_type}.json", "w") as myfile:
             myfile.write(json.dumps(commands))
 
@@ -205,13 +206,22 @@ class SocketRobotArm:
 
         position[1] += 0.02
 
+        json_res = []
+
         for pos in [conv, position]:
             jointPoses = self.convert_to_joint_base(
                 pos,
                 np.radians([0, 0, -90]),
             )
+            json_res.append(
+                self.prepare_data.make_step(
+                    "free", np.degrees(jointPoses), [0, 0, 0, 0, 0, 0]
+                )
+            )
             self.motionFund(jointPoses)
             time.sleep(2)
+        with open(f"data/palletizing.json", "w") as myfile:
+            myfile.write(json.dumps(json_res))
 
         end_effector_state = p.getLinkState(self.body_id, self.num_joints - 1)
         end_effector_pos = end_effector_state[0]  # Позиция наконечника
@@ -227,14 +237,15 @@ class SocketRobotArm:
         # Создаём констрейнт для прикрепления
         constraint_id = p.createConstraint(
             parentBodyUniqueId=self.body_id,  # Идентификатор робота
-            parentLinkIndex=self.num_joints - 1,  # Индекс последнего звена (наконечника)
+            parentLinkIndex=self.num_joints
+            - 1,  # Индекс последнего звена (наконечника)
             childBodyUniqueId=box_id,  # Идентификатор коробки
             childLinkIndex=-1,  # -1 означает базовую ссылку объекта
             jointType=p.JOINT_FIXED,  # Фиксированный констрейнт
             jointAxis=[0, 0, 0],  # Без вращательной оси
             parentFramePosition=parentFramePosition,  # Позиция относительно робота
             childFramePosition=[1, 1, 1],  # Позиция объекта (относительно себя)
-            parentFrameOrientation=parentFrameOrientation  # Ориентация относительно робота
+            parentFrameOrientation=parentFrameOrientation,  # Ориентация относительно робота
         )
 
     def set_text(self, text):
@@ -316,6 +327,11 @@ class SocketRobotArm:
             jointPoses = [np.radians(float(point[f"m{i}"])) for i in range(6)]
             self.motionFund(jointPoses)
             
+    def get_palletizing_json(self):
+        with open(f"data/palletizing.json", "r") as fp:
+            data = json.load(fp)
+        return data
+
     def convert_point_to_free(self):
         points = self.steps_from_file()
         res = []