diff --git a/robot/conveyor.py b/robot/conveyor.py new file mode 100644 index 0000000..5b20564 --- /dev/null +++ b/robot/conveyor.py @@ -0,0 +1,119 @@ +import pybullet as p +import time +import threading + + +class ConveyorBelt: + def __init__(self, size, position, box_size, box_spacing, box_speed): + self.conveyor_size = size + self.conveyor_position = position + self.conveyor_id = self.create_conveyor(size, position) + self.box_size = box_size + self.box_spacing = box_spacing + self.box_speed = box_speed + self.boxes = [] + self.create_boxes() + self.stop_thread = False + self.thread = threading.Thread(target=self.run) + self.thread.start() + + def create_conveyor(self, size, position): + collision_shape_id = p.createCollisionShape( + shapeType=p.GEOM_BOX, + halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0], + ) + visual_shape_id = p.createVisualShape( + shapeType=p.GEOM_BOX, + halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0], + rgbaColor=[0.5, 0.5, 0.5, 1], + ) + conveyor_id = p.createMultiBody( + baseMass=0.0, + baseCollisionShapeIndex=collision_shape_id, + baseVisualShapeIndex=visual_shape_id, + basePosition=position, + ) + return conveyor_id + + def box_position(self, i=0): + x_position = ( + self.conveyor_position[0] + - self.conveyor_size[0] * 0.5 + + i * (self.box_size[0] + self.box_spacing) + ) + y_position = ( + self.conveyor_position[1] + self.conveyor_size[1] * 0.5 - self.box_size[1] + ) + z_position = ( + self.conveyor_position[2] + self.conveyor_size[2] + self.box_size[2] * 0.5 + ) + return [x_position, y_position, z_position] + + def create_boxes(self): + num_boxes = int(self.conveyor_size[0] / (self.box_size[0] + self.box_spacing)) + for i in range(num_boxes): + position = self.box_position(i) + self.boxes.append(self.create_box(position)) + + def create_box(self, position): + collision_shape_id = p.createCollisionShape( + shapeType=p.GEOM_BOX, + halfExtents=[ + self.box_size[0] / 2.0, + self.box_size[1] / 2.0, + self.box_size[2] / 2.0, + ], + ) + visual_shape_id = p.createVisualShape( + shapeType=p.GEOM_BOX, + halfExtents=[ + self.box_size[0] / 2.0, + self.box_size[1] / 2.0, + self.box_size[2] / 2.0, + ], + rgbaColor=[1, 0, 0, 1], + ) + box_id = p.createMultiBody( + baseMass=1.0, + baseCollisionShapeIndex=collision_shape_id, + baseVisualShapeIndex=visual_shape_id, + basePosition=position, + baseOrientation=p.getQuaternionFromEuler([0, 0, 0]), + ) + return box_id + + def move_boxes(self): + # Двигаем коробки вдоль конвейера + for box_id in self.boxes[:]: + # Получаем текущую позицию коробки + pos, orn = p.getBasePositionAndOrientation(box_id) + + # Проверяем, если коробка выходит за пределы конвейера по оси X + # Начальное положение коробки + её длина (по оси X) + if ( + pos[0] + self.box_size[0] * 0.5 + > self.conveyor_position[0] + self.conveyor_size[0] * 0.5 + ): + # Если коробка выходит за конец конвейера, она падает вниз (физика продолжает действовать) + # Удаляем коробку с конвейера + self.boxes.remove(box_id) + else: + # Двигаем коробку вдоль конвейера (по оси X) + new_pos = [pos[0] + self.box_speed, pos[1], pos[2]] + p.resetBasePositionAndOrientation(box_id, new_pos, orn) + + # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество + if len(self.boxes) < int( + self.conveyor_size[0] / (self.box_size[0] + self.box_spacing) + ): + new_box = self.create_box(self.box_position()) + self.boxes.append(new_box) + + def run(self): + while not self.stop_thread: + self.move_boxes() + time.sleep(1) + + def stop(self): + self.stop_thread = True + self.thread.join() diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py index da180c5..ab83f7a 100644 --- a/robot/urdf_manager.py +++ b/robot/urdf_manager.py @@ -1,19 +1,13 @@ -import socket -import json import os -import math import numpy as np import time -from typing import Literal -from pprint import pprint - -import threading import pybullet as p -import pybullet_industrial as pi from logger import logger +from robot.conveyor import ConveyorBelt + class UrdfManager: urdf_filename = None @@ -35,65 +29,21 @@ class UrdfManager: self.physics_client = p.connect(getattr(p, type)) logger.info(f"Connect to {self.physics_client} by {type}") - def create_cube(self, size, position): - # Создание формы коллизии для куба - collision_shape_id = p.createCollisionShape( - shapeType=p.GEOM_BOX, - halfExtents=[ - size[0] / 2.0, - size[1] / 2.0, - size[2] / 2.0, - ], # halfExtents задаются радиусами по осям x, y и z - ) - - # Создание визуальной формы для куба - visual_shape_id = p.createVisualShape( - shapeType=p.GEOM_BOX, - halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0], - rgbaColor=[0, 0, 1, 1], # Синий цвет - ) - - # Создание многосоставного тела с формой коллизии и визуальной формой - body_id = p.createMultiBody( - baseMass=1.0, # Масса тела - baseCollisionShapeIndex=collision_shape_id, - baseVisualShapeIndex=visual_shape_id, - basePosition=position, - ) - - return body_id - - def create_conveyor(self, size, position): - collision_shape_id = p.createCollisionShape( - shapeType=p.GEOM_BOX, - halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0], - ) - # Создание визуальной формы для конвейера - visual_shape_id = p.createVisualShape( - shapeType=p.GEOM_BOX, - halfExtents=[size[0] / 2.0, size[1] / 2.0, size[2] / 2.0], - rgbaColor=[0.5, 0.5, 0.5, 1], # Серый цвет для конвейера - ) - - # Создание тела для конвейера (можно без массы, если оно не должно взаимодействовать с другими объектами) - conveyor_id = p.createMultiBody( - baseMass=0.0, # Это статичный объект, поэтому масса 0 - baseCollisionShapeIndex=collision_shape_id, - baseVisualShapeIndex=visual_shape_id, - basePosition=position, - ) - - return conveyor_id - def load_models(self): - # p.loadURDF("urdf/plane.urdf", physicsClientId=self.physics_client) + 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 + urdf_path, + self.robot_start_position, + useFixedBase=True ) + p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client) def load_palletizing(self): # Путь к URDF модели палеты @@ -116,42 +66,17 @@ class UrdfManager: / 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 - ) + # self.pallet_id = p.loadURDF( + # urdf_path_pallet, pallet_position, pallet_orientation, useFixedBase=True + # ) - # Создание коробок на палете - box_size = [0.2, 0.2, 0.2] # Размеры коробки: [ширина, высота, глубина] - boxes_per_row = 3 - boxes_per_column = 2 + 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 # Скорость движения коробок - for row in range(boxes_per_column): - for col in range(boxes_per_row): - box_x_offset = col * (box_size[0] + 0.05) - box_y_offset = row * (box_size[1] + 0.05) - box_z_position = pallet_height + (box_size[2] / 2) - - box_position = [ - pallet_position[0] - + box_x_offset - - (boxes_per_row * (box_size[0] + 0.05) / 2), - pallet_position[1] - + box_y_offset - - (boxes_per_column * (box_size[1] + 0.05) / 2), - box_z_position, - ] - - # Создаем кубик - self.create_cube(box_size, box_position) - - # Создание конвейера - conveyor_size = [3.0, 0.5, 0.1] # Длина, ширина и высота - conveyor_position = [ - 0.5, - 1.5, - 0.5, - ] # Позиция в пространстве (выше уровня земли) - self.conveyor_id = self.create_conveyor(conveyor_size, conveyor_position) + ConveyorBelt(conveyor_size, conveyor_position, box_size, box_spacing, box_speed) time.sleep(1)