174 lines
5.3 KiB
Python
174 lines
5.3 KiB
Python
import os
|
||
import sys
|
||
|
||
project_base_dir = os.path.abspath(os.path.join(
|
||
os.path.dirname(os.path.abspath(__file__)), './')) + '/assembly/'
|
||
|
||
|
||
sys.path.append(project_base_dir)
|
||
sys.path.append(project_base_dir + '/baselines/')
|
||
sys.path.append(project_base_dir + '/assets/')
|
||
|
||
from scipy.spatial.transform import Rotation
|
||
import shutil
|
||
from spatialmath import *
|
||
from spatialmath.base import *
|
||
from assembly.assets.process_mesh import process_mesh
|
||
from assembly.examples.run_joint_plan import get_planner
|
||
from assembly.baselines.run_joint_plan import PyPlanner
|
||
from assembly.assets.subdivide import subdivide_to_size
|
||
import numpy as np
|
||
import json
|
||
import trimesh
|
||
|
||
import re
|
||
def merge_meshes(meshes):
|
||
# Создание пустого меша
|
||
merged_mesh = trimesh.Trimesh()
|
||
|
||
# Объединение каждого меша в один
|
||
for mesh in meshes:
|
||
merged_mesh = trimesh.util.concatenate(
|
||
[merged_mesh, trimesh.load(mesh)])
|
||
i = True
|
||
while i:
|
||
if merged_mesh.fill_holes():
|
||
i = False
|
||
|
||
|
||
|
||
|
||
|
||
return merged_mesh
|
||
|
||
|
||
os.environ['OMP_NUM_THREADS'] = '1'
|
||
|
||
|
||
|
||
class FS:
|
||
def readJSON(path: str):
|
||
return json.loads((open(path)).read())
|
||
|
||
def writeFile(data, filePath, fileName):
|
||
|
||
file_to_open = filePath + fileName
|
||
|
||
f = open(file_to_open, 'w', )
|
||
|
||
f.write(data)
|
||
|
||
def readFile(path: str):
|
||
return open(path).read()
|
||
|
||
def readFilesTypeFolder(pathFolder: str, fileType='.json'):
|
||
return os.listdir(pathFolder)
|
||
|
||
def readFolder(pathFolder: str):
|
||
return list(map(lambda el: pathFolder + '/' + el, os.listdir(pathFolder)))
|
||
|
||
def createFolder(path: str):
|
||
if (not os.path.exists(path)):
|
||
return os.mkdir(path)
|
||
|
||
|
||
def listGetFirstValue(iterable, default=False, pred=None):
|
||
return next(filter(pred, iterable), default)
|
||
|
||
|
||
def filterModels(filterModels, filterModelsDescription):
|
||
models = []
|
||
for el in filterModelsDescription:
|
||
models.append(listGetFirstValue(
|
||
filterModels, None, lambda x: x.name == el))
|
||
return models
|
||
|
||
|
||
# mesh1 = trimesh.load('/Users/idontsudo/framework/asp/out/sdf-generation/meshes/Cube.obj')
|
||
# mesh2 = trimesh.load('/Users/idontsudo/framework/asp/out/sdf-generation/meshes/Cube001.obj')
|
||
|
||
|
||
# # Объединение мешей
|
||
# merged_mesh = merge_meshes([mesh1, mesh2])
|
||
|
||
# # Сохранение объединенного меша в файл
|
||
# merged_mesh.export('merged.obj')
|
||
def main():
|
||
# from argparse import ArgumentParser
|
||
# parser = ArgumentParser()
|
||
# parser.add_argument('--asp-path', type=str, required=True)
|
||
# args = parser.parse_args()
|
||
# aspDir = args.asp_dir
|
||
|
||
# # Коректировка пути до папки с генерацией ASP
|
||
# if (aspDir == None):
|
||
# args.print_helper()
|
||
# if (aspDir[aspDir.__len__() - 1] != '/'):
|
||
# aspDir += '/'
|
||
aspDir = '/home/idontsudo/framework/asp/out/'
|
||
sequences = FS.readJSON(aspDir + 'sequences.json').get('sequences')
|
||
|
||
assemblyDirNormalize = []
|
||
for el in FS.readFolder(aspDir + 'assemblys'):
|
||
for e in FS.readFolder(el):
|
||
try:
|
||
# Пост обработка .obj обьектов
|
||
process_mesh(source_dir=e, target_dir=e +
|
||
'/process/', subdivide=e, verbose=True)
|
||
assemblyDirNormalize.append(e + '/process/')
|
||
except Exception as e:
|
||
print('ERRROR:')
|
||
print(e)
|
||
|
||
|
||
print(assemblyDirNormalize)
|
||
for el in assemblyDirNormalize:
|
||
asset_folder = os.path.join(project_base_dir, aspDir)
|
||
assembly_dir = os.path.join(asset_folder, el)
|
||
planner = get_planner('bfs')(assembly_dir, assembly_dir, 0, [
|
||
1], False, 'sdf', 0.05, 0.01, 100, 100, True)
|
||
|
||
# Планирование пути
|
||
status, t_plan, path = planner.plan(
|
||
120, seed=1, return_path=True, render=False, record_path=None
|
||
)
|
||
coords = []
|
||
|
||
for k in path:
|
||
seMatrix = SE3(k)
|
||
euler = seMatrix.eul()
|
||
coord = seMatrix.A[0:3, 3]
|
||
rot = Rotation.from_euler('xyz', euler, degrees=True).as_quat()
|
||
coords.append({'quadrelion': [rot[0], rot[1], rot[2], rot[3]], 'xyz': [
|
||
coord[0], coord[1], coord[2]], 'euler': [euler[0], euler[1], euler[2]]})
|
||
# Запись пути в кортеж
|
||
planingObject = {
|
||
"time": t_plan,
|
||
"insertion_path": coords,
|
||
"status": status,
|
||
}
|
||
# Запись результата планирования
|
||
FS.writeFile(json.dumps(planingObject),
|
||
el[0:el.__len__() - 8], 'insertion_path.json')
|
||
|
||
try:
|
||
planner = PyPlanner(assembly_dir, 'process', still_ids=[1],)
|
||
status, t_plan, path = planner.plan(
|
||
planner_name='rrt',
|
||
step_size=None,
|
||
max_time=None,
|
||
seed=1,
|
||
return_path=True,
|
||
simplify=False,
|
||
render=False
|
||
)
|
||
|
||
print(f'Status: {status}, planning time: {t_plan}')
|
||
|
||
if args.save_dir is not None:
|
||
planner.save_path(path, args.save_dir, args.n_save_state)
|
||
except Exception as e:
|
||
print(e)
|
||
|
||
|
||
main()
|