normal loop
This commit is contained in:
parent
5a0ef0082c
commit
10544e4308
|
|
@ -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
|
||||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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"]
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
Loading…
Reference in New Issue