pack lib
This commit is contained in:
parent
10544e4308
commit
b00e8d46b2
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue