diff --git a/packer/packer.py b/packer/packer.py new file mode 100644 index 0000000..4b7c896 --- /dev/null +++ b/packer/packer.py @@ -0,0 +1,41 @@ +from py3dbp import Packer, Bin, Item + +class PalletPacker: + def __init__(self, pallet: dict, box: dict): + self.pallet_width = pallet.get('x', 1.2) + self.pallet_height = pallet.get('y', 0.8) + self.pallet_depth = pallet.get('z', 0.144) + + self.box_width = box.get('x', 0.2) + self.box_height = box.get('y', 0.2) + self.box_depth = box.get('z', 0.2) + + def pack(self): + # Создаем контейнер (паллета) + pallet = Bin('pallet', self.pallet_width, self.pallet_height, self.box_depth, self.pallet_width) + + # Создаем пакер + packer = Packer() + + # Вычисляем, сколько коробок поместится на паллете в один слой + num_boxes_x = self.pallet_width // self.box_width + num_boxes_y = self.pallet_height // self.box_height + num_boxes = int(num_boxes_x * num_boxes_y) + + # Добавляем коробки в пакер + for i in range(num_boxes): + packer.add_item(Item(f'box{i}', self.box_width, self.box_height, self.box_depth, 1)) + + # Добавляем контейнер в пакер + packer.add_bin(pallet) + + # Упаковываем коробки + packer.pack() + + # Получаем результат + coordinates = [] + for b in packer.bins: + for item in b.items: + coordinates.append((item.position, item.width, item.height, item.depth)) + + return coordinates diff --git a/poetry.lock b/poetry.lock index 276a85e..ca1010b 100644 --- a/poetry.lock +++ b/poetry.lock @@ -191,6 +191,16 @@ files = [ dev = ["black", "check-manifest", "coverage", "packaging", "pylint", "pyperf", "pypinfo", "pytest-cov", "requests", "rstcheck", "ruff", "sphinx", "sphinx_rtd_theme", "toml-sort", "twine", "virtualenv", "wheel"] test = ["pytest", "pytest-xdist", "setuptools"] +[[package]] +name = "py3dbp" +version = "1.1.2" +description = "3D Bin Packing" +optional = false +python-versions = "*" +files = [ + {file = "py3dbp-1.1.2.tar.gz", hash = "sha256:d8c92c120eecfbee600a4dfd8b0a42d63459c97de67a26ff8ad39529a5125e07"}, +] + [[package]] name = "pybullet" version = "3.2.6" @@ -390,4 +400,4 @@ files = [ [metadata] lock-version = "2.0" python-versions = "^3.10.11" -content-hash = "e1d27064cea05111c01747d08f2493a4213f0dbf1d274aa513ffd33ca43a87d7" +content-hash = "6d16510fd35519dee80acbeb09936403172391cc840df3b5be874e6eab27f242" diff --git a/pyproject.toml b/pyproject.toml index 76a65e8..ebf60ff 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -18,6 +18,7 @@ pyqt6 = "^6.7.1" setuptools = "^75.3.0" psutil = "^6.1.0" pybullet-industrial = "^1.0.3" +py3dbp = "^1.1.2" [build-system] requires = ["poetry-core"] diff --git a/robot/client_socket.py b/robot/client_socket.py index 50b4050..846e90c 100644 --- a/robot/client_socket.py +++ b/robot/client_socket.py @@ -62,6 +62,7 @@ class SocketRobotArm: application_path, self.palletizing, ) + self.constraint_id = None def __exit__(self, exc_type, exc_value, traceback): logger.info("exiting") @@ -206,56 +207,61 @@ class SocketRobotArm: def palletizing(self, position, box_id): dir_wall = (self.urdf_manager.conv_direction + 1) % 2 - conv = position[:] - conv[dir_wall] -= 0.4 - position[dir_wall] -= 0.02 + near_position = position[:] + near_position[dir_wall] -= 0.4 + + object_position = position[:] + object_position[dir_wall] -= 0.02 + + pallet_position = self.urdf_manager.pallet.position + + base_position = pallet_position[:] + base_position[2] += 1.0 json_res = [] tool_pos = [0, 0, 0] + + steps = [ + {'type': 'move', 'coordinates': near_position}, + {'type': 'move', 'coordinates': object_position}, + {'type': 'create_constraint'}, + {'type': 'move', 'coordinates': pallet_position}, + {'type': 'remove_constraint'}, + {'type': 'move', 'coordinates': base_position}, + ] + constraint_id = None - for pos in [conv, position]: - 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] + for step in steps: + if step['type'] == 'move': + jointPoses = self.convert_to_joint_base( + step['coordinates'], + np.radians(tool_pos), ) - ) - self.motionFund(jointPoses) - # time.sleep(2) + json_res.append( + self.prepare_data.make_step( + "free", np.degrees(jointPoses), [0, 0, 0, 0, 0, 0] + ) + ) + self.motionFund(jointPoses) + if step['type'] == 'create_constraint': + constraint_id = p.createConstraint( + parentBodyUniqueId=self.body_id, + parentLinkIndex=self.num_joints - 1, + childBodyUniqueId=box_id, + childLinkIndex=-1, + jointType=p.JOINT_FIXED, + jointAxis=[0, 0, 0], + parentFramePosition=[0, 0, 0], + childFramePosition=[0, 0.2, 0], + parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 90])), + childFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])), + ) + if step['type'] == 'remove_constraint': + p.removeConstraint(constraint_id) with open(f"data/palletizing.json", "w") as myfile: myfile.write(json.dumps(json_res)) - - # Создаём констрейнт для прикрепления - constraint_id = p.createConstraint( - parentBodyUniqueId=self.body_id, - parentLinkIndex=self.num_joints - 1, - childBodyUniqueId=box_id, - childLinkIndex=-1, - jointType=p.JOINT_FIXED, - jointAxis=[0, 0, 0], - parentFramePosition=[0, 0, 0], - childFramePosition=[0, 0.2, 0], - 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): diff --git a/robot/conveyor.py b/robot/conveyor.py index 0ee209b..0d401b8 100644 --- a/robot/conveyor.py +++ b/robot/conveyor.py @@ -115,9 +115,10 @@ class ConveyorBelt: p.resetBasePositionAndOrientation(box_id, new_pos, orn) # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество - if len(self.boxes) < int( - self.conveyor_size[0] / (self.box_size[0] + self.box_spacing) - ): + num_boxes = int( + self.conveyor_size[self.direction] / (self.box_size[0] + self.box_spacing) + ) + if len(self.boxes) < num_boxes: new_box = self.create_box(self.box_position()) self.boxes.append(new_box) diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py index 3e447b7..eef7b3a 100644 --- a/robot/urdf_manager.py +++ b/robot/urdf_manager.py @@ -8,6 +8,7 @@ from logger import logger from robot.conveyor import ConveyorBelt from robot.pallet import Pallet +from packer.packer import PalletPacker class UrdfManager: @@ -20,6 +21,9 @@ class UrdfManager: self.robot_start_position = robot_start_position self.application_path = application_path self.palletizing = palletizing + + self.conveyor = None + self.pallet = None def start_loop(self, urdf): self.urdf_filename = urdf @@ -27,6 +31,7 @@ class UrdfManager: p.resetSimulation() self.load_models() p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client) + self.state_id = p.saveState() def run_pybullet(self, type="DIRECT"): self.physics_client = p.connect(getattr(p, type)) @@ -48,8 +53,14 @@ class UrdfManager: ) def load_palletizing(self): - # p.resetSimulation() - self.pallet = Pallet([1.3, 0.6, 0.1]) + if self.conveyor: + self.conveyor = None + if self.pallet: + self.pallet = None + self.start_loop(self.urdf_filename) + + self.pallet_size = [1.3, 0.6, 0.1] + self.pallet = Pallet(self.pallet_size) # положение дальнего угла self.conv_target = [1.1, -0.63, 0.4] @@ -59,16 +70,31 @@ class UrdfManager: (self.conv_target[i] - self.conv_size[i] * 0.5) for i in range(len(self.conv_target)) ] + self.box_size = [0.2, 0.2, 0.2] self.conveyor = ConveyorBelt( self.conv_size, self.conv_pos, self.conv_direction, - [0.2, 0.2, 0.2], + self.box_size, 0.2, 0.1, self.palletizing, ) + self.packing = PalletPacker( + { + "x": self.pallet_size[0], + "y": self.pallet_size[1], + "z": self.pallet_size[2], + }, + { + "x": self.box_size[0], + "y": self.box_size[1], + "z": self.box_size[2], + }, + ) + logger.info(self.packing.pack()) + time.sleep(1) def get_pybullet_image(self):