diff --git a/packer/packer.py b/packer/packer.py index 4b7c896..73ab84e 100644 --- a/packer/packer.py +++ b/packer/packer.py @@ -1,21 +1,26 @@ from py3dbp import Packer, Bin, Item +from logger import logger + + 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) + self.pallet_width = pallet.get("x", 1.2) + self.pallet_height = pallet.get("y", 0.8) + self.pallet_depth = pallet.get("z", 0.144) + + gap = 0.2 + self.box_width = box.get("x", 0.2) + gap + self.box_height = box.get("y", 0.2) + gap + 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() + packer.add_bin( + Bin( + "pallet", self.pallet_width, self.pallet_height, self.pallet_depth, 1000 + ) + ) # Вычисляем, сколько коробок поместится на паллете в один слой num_boxes_x = self.pallet_width // self.box_width @@ -24,10 +29,9 @@ class PalletPacker: # Добавляем коробки в пакер 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.add_item( + Item(f"box{i}", self.box_width, self.box_height, self.box_depth, 1) + ) # Упаковываем коробки packer.pack() @@ -36,6 +40,6 @@ class PalletPacker: coordinates = [] for b in packer.bins: for item in b.items: - coordinates.append((item.position, item.width, item.height, item.depth)) + coordinates.append([float(i) for i in item.position]) return coordinates diff --git a/robot/client_socket.py b/robot/client_socket.py index 846e90c..fcdef81 100644 --- a/robot/client_socket.py +++ b/robot/client_socket.py @@ -204,37 +204,40 @@ class SocketRobotArm: def imitate(self): threading.Thread(target=self.imitate_func, daemon=True).start() - def palletizing(self, position, box_id): + def palletizing(self, position, box_id, counter): dir_wall = (self.urdf_manager.conv_direction + 1) % 2 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 - + + pallet_position = self.urdf_manager.packing[ + counter % len(self.urdf_manager.packing) + ] + logger.info(counter) + logger.info(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}, + {"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 step in steps: - if step['type'] == 'move': + if step["type"] == "move": jointPoses = self.convert_to_joint_base( - step['coordinates'], + step["coordinates"], np.radians(tool_pos), ) json_res.append( @@ -243,7 +246,7 @@ class SocketRobotArm: ) ) self.motionFund(jointPoses) - if step['type'] == 'create_constraint': + if step["type"] == "create_constraint": constraint_id = p.createConstraint( parentBodyUniqueId=self.body_id, parentLinkIndex=self.num_joints - 1, @@ -253,15 +256,19 @@ class SocketRobotArm: 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])), + parentFrameOrientation=p.getQuaternionFromEuler( + np.radians([0, 0, 90]) + ), + childFrameOrientation=p.getQuaternionFromEuler( + np.radians([0, 0, 0]) + ), ) - if step['type'] == 'remove_constraint': + if step["type"] == "remove_constraint": p.removeConstraint(constraint_id) with open(f"data/palletizing.json", "w") as myfile: myfile.write(json.dumps(json_res)) - + self.urdf_manager.conveyor.conveyor_stopped = False def set_text(self, text): diff --git a/robot/conveyor.py b/robot/conveyor.py index 0d401b8..7d90067 100644 --- a/robot/conveyor.py +++ b/robot/conveyor.py @@ -28,6 +28,7 @@ class ConveyorBelt: self.box_spacing = box_spacing self.box_speed = box_speed self.boxes = [] + self.box_counter = 0 self.create_boxes() self.stop_thread = False @@ -83,7 +84,7 @@ class ConveyorBelt: rgbaColor=[1, 0, 0, 1], ) box_id = p.createMultiBody( - baseMass=1.0, + baseMass=10.0, baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, basePosition=position, @@ -107,8 +108,9 @@ class ConveyorBelt: wall_position = list(pos) dir_wall = (self.direction + 1) % (3 - 1) wall_position[dir_wall] -= self.box_size[dir_wall] * 0.5 - - self.call_robot(wall_position, box_id) + + self.call_robot(wall_position, box_id, self.box_counter) + self.box_counter += 1 else: new_pos = list(pos[:]) new_pos[self.direction] += self.box_speed @@ -122,10 +124,10 @@ class ConveyorBelt: new_box = self.create_box(self.box_position()) self.boxes.append(new_box) - def call_robot(self, position, box_id): + def call_robot(self, position, box_id, counter): # Здесь можно вызвать робота для выполнения операции, например: logger.info("Конвейер остановлен, вызываем робота для работы с коробкой.") - self.palletizingFunc(position, box_id) + self.palletizingFunc(position, box_id, counter) # time.sleep(5) # self.conveyor_stopped = False diff --git a/robot/pallet.py b/robot/pallet.py index 1741eb8..74977a7 100644 --- a/robot/pallet.py +++ b/robot/pallet.py @@ -3,13 +3,10 @@ import pybullet as p class Pallet: def __init__( - self, target=[1, 1, 0], size=[0.8, 1.2, 0.144], color=[0.6, 0.3, 0.1, 0.5] + self, position=[1, 1, 0], size=[0.8, 1.2, 0.144], color=[0.6, 0.3, 0.1, 0.5] ): self.size = size - self.position = [ - (target[i] - self.size[i] * 0.5) - for i in range(len(target)) - ] + self.position = position self.color = color self.create_pallet() diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py index eef7b3a..03fe4b9 100644 --- a/robot/urdf_manager.py +++ b/robot/urdf_manager.py @@ -21,7 +21,7 @@ class UrdfManager: self.robot_start_position = robot_start_position self.application_path = application_path self.palletizing = palletizing - + self.conveyor = None self.pallet = None @@ -59,18 +59,24 @@ class UrdfManager: 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.pallet_target = [1.3 + 0.5, 0.6, 0.1] + self.pallet_size = [0.8, 1.2, 0.144] + self.pallet_position = [ + (self.pallet_target[i] - self.pallet_size[i] * 0.5) + for i in range(len(self.pallet_target)) + ] + self.pallet = Pallet(self.pallet_position) + self.box_size = [0.2, 0.2, 0.2] + # положение дальнего угла - self.conv_target = [1.1, -0.63, 0.4] + self.conv_target = [1.1 + 0.5, -0.63, 0.4] self.conv_size = [0.5, 3.0, 0.1] self.conv_direction = self.conv_size.index(max(self.conv_size)) self.conv_pos = [ (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, @@ -85,15 +91,18 @@ class UrdfManager: { "x": self.pallet_size[0], "y": self.pallet_size[1], - "z": self.pallet_size[2], + "z": self.box_size[2], # высота паллеты в один слой }, { "x": self.box_size[0], "y": self.box_size[1], "z": self.box_size[2], }, - ) - logger.info(self.packing.pack()) + ).pack() + logger.info(self.packing) + for p in self.packing: + for i in range(len(self.box_size)): + p[i] = (self.pallet_position[i] - self.pallet_size[i] * 0.5) + self.box_size[i] * 0.5 - p[i] time.sleep(1)