diff --git a/gui/command.py b/gui/command.py index 1a65f99..e82bd59 100644 --- a/gui/command.py +++ b/gui/command.py @@ -12,12 +12,12 @@ class Command(QWidget): self.command_type = command_type self.startCommand = startCommand self.changeType = changeType - + self.initUI() - + self.timer = QTimer() self.timer.timeout.connect(self.updateState) - self.timer.start(int(100)) + self.timer.start(int(100)) def initUI(self): self.layout = QVBoxLayout() @@ -33,9 +33,14 @@ class Command(QWidget): self.updButton = QPushButton("Запустить single cycle") self.updButton.clicked.connect(self.startCommand) 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): @@ -44,7 +49,7 @@ class Command(QWidget): p = self.palette() p.setColor(self.backgroundRole(), Qt.GlobalColor.lightGray) self.setPalette(p) - + def updateState(self): self.stepLabel.setText(f"{self.command_data()}") - self.commandLabel.setText(f"Тип загрузки {self.command_type()}") \ No newline at end of file + self.commandLabel.setText(f"Тип загрузки {self.command_type()}") diff --git a/gui/init.py b/gui/init.py index 0dfba7e..1aa4572 100644 --- a/gui/init.py +++ b/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), ) diff --git a/robot/client_socket.py b/robot/client_socket.py index 484b66a..66935a1 100644 --- a/robot/client_socket.py +++ b/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)) @@ -202,17 +203,26 @@ class SocketRobotArm: def palletizing(self, position, box_id): conv = position[:] conv[1] += 0.4 - + 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] # Позиция наконечника end_effector_orn = end_effector_state[1] # Ориентация наконечника @@ -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): @@ -315,6 +326,11 @@ class SocketRobotArm: for point in points: 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()