104 lines
3.6 KiB
Python
104 lines
3.6 KiB
Python
import os
|
||
import numpy as np
|
||
import time
|
||
|
||
import pybullet as p
|
||
|
||
from logger import logger
|
||
|
||
from robot.conveyor import ConveyorBelt
|
||
|
||
|
||
class UrdfManager:
|
||
urdf_filename = None
|
||
urdf_filename_tool = "welding_gun"
|
||
physics_client = None
|
||
body_id = None
|
||
|
||
def __init__(self, robot_start_position, application_path):
|
||
self.robot_start_position = robot_start_position
|
||
self.application_path = application_path
|
||
|
||
def start_loop(self, urdf):
|
||
self.urdf_filename = urdf
|
||
|
||
p.resetSimulation()
|
||
self.load_models()
|
||
|
||
def run_pybullet(self, type="DIRECT"):
|
||
self.physics_client = p.connect(getattr(p, type))
|
||
logger.info(f"Connect to {self.physics_client} by {type}")
|
||
|
||
def load_models(self):
|
||
p.loadURDF(
|
||
os.path.join(self.application_path, "urdf", "plane.urdf"),
|
||
physicsClientId=self.physics_client,
|
||
)
|
||
|
||
urdf_path = os.path.join(
|
||
self.application_path, "urdf", f"{self.urdf_filename}.urdf"
|
||
)
|
||
self.body_id = p.loadURDF(
|
||
urdf_path,
|
||
self.robot_start_position,
|
||
useFixedBase=True
|
||
)
|
||
p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
|
||
|
||
def load_palletizing(self):
|
||
# Путь к URDF модели палеты
|
||
urdf_path_pallet = os.path.join(
|
||
self.application_path,
|
||
"urdf",
|
||
"europallet.urdf", # Убедитесь, что имя файла соответствует
|
||
)
|
||
|
||
# Загрузка палеты рядом с роботом
|
||
# Предположим, что вы хотите расположить палету на расстоянии от робота
|
||
pallet_offset_x = 1.5 # Расстояние в метрах по оси X
|
||
pallet_offset_y = 0.0 # Расстояние по оси Y
|
||
pallet_height = 0.145 # Высота палеты (подгонка под высоту робота)
|
||
|
||
pallet_position = [
|
||
self.robot_start_position[0] + pallet_offset_x,
|
||
self.robot_start_position[1] + pallet_offset_y,
|
||
pallet_height
|
||
/ 2.0, # Разместите палету выше, чтобы она не находилась на земле
|
||
]
|
||
pallet_orientation = p.getQuaternionFromEuler([0, 0, np.deg2rad(90)])
|
||
# self.pallet_id = p.loadURDF(
|
||
# urdf_path_pallet, pallet_position, pallet_orientation, useFixedBase=True
|
||
# )
|
||
|
||
conveyor_size = [3.0, 0.5, 0.1]
|
||
conveyor_position = [0.5, 1.5, 0.5]
|
||
box_size = [0.2, 0.2, 0.2]
|
||
box_spacing = 0.2
|
||
box_speed = 0.1 # Скорость движения коробок
|
||
|
||
ConveyorBelt(conveyor_size, conveyor_position, box_size, box_spacing, box_speed)
|
||
|
||
time.sleep(1)
|
||
|
||
def get_pybullet_image(self):
|
||
if self.physics_client is None:
|
||
return
|
||
|
||
width, height, rgb, _, _ = p.getCameraImage(
|
||
width=500,
|
||
height=500,
|
||
viewMatrix=p.computeViewMatrix(
|
||
cameraEyePosition=[4, -4, 1.5],
|
||
# cameraEyePosition=[4,-4,4],
|
||
cameraTargetPosition=[0, 0, 0], # Центр фокусировки камеры
|
||
cameraUpVector=[0, 0, 1], # Направление вверх
|
||
),
|
||
projectionMatrix=p.computeProjectionMatrixFOV(
|
||
fov=60.0, aspect=1.0, nearVal=0.1, farVal=10.0
|
||
),
|
||
# renderer=p.ER_TINY_RENDERER,
|
||
physicsClientId=self.physics_client,
|
||
)
|
||
|
||
return (rgb, width, height)
|