conv visualize
This commit is contained in:
parent
886a9d0e5a
commit
31d1aff906
|
|
@ -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()
|
||||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue