adding comments
This commit is contained in:
IDONTSUDO
parent
9eaf12a4fa
commit
12482ff40f
7 changed files with 102 additions and 75 deletions
|
|
@ -1,25 +1,25 @@
|
|||
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 sys
|
||||
import os
|
||||
os.environ['OMP_NUM_THREADS'] = '1'
|
||||
import sys
|
||||
|
||||
project_base_dir = os.path.abspath(os.path.join(os.path.dirname(os.path.abspath(__file__)), './')) + '/assembly/'
|
||||
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/')
|
||||
|
||||
import json
|
||||
import numpy as np
|
||||
from assembly.assets.subdivide import subdivide_to_size
|
||||
from assembly.baselines.run_joint_plan import PyPlanner
|
||||
from assembly.examples.run_joint_plan import get_planner
|
||||
from assembly.assets.process_mesh import process_mesh
|
||||
|
||||
from spatialmath.base import *
|
||||
from spatialmath import *
|
||||
import shutil
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
class FS:
|
||||
def readJSON(path: str):
|
||||
|
|
@ -38,9 +38,10 @@ class FS:
|
|||
|
||||
def readFilesTypeFolder(pathFolder: str, fileType='.json'):
|
||||
return os.listdir(pathFolder)
|
||||
|
||||
def readFolder(pathFolder: str):
|
||||
return list( map(lambda el: pathFolder + '/' + el, os.listdir(pathFolder) ))
|
||||
|
||||
return list(map(lambda el: pathFolder + '/' + el, os.listdir(pathFolder)))
|
||||
|
||||
|
||||
def listGetFirstValue(iterable, default=False, pred=None):
|
||||
return next(filter(pred, iterable), default)
|
||||
|
|
@ -52,68 +53,80 @@ def filterModels(filterModels, filterModelsDescription):
|
|||
models.append(listGetFirstValue(
|
||||
filterModels, None, lambda x: x.name == el))
|
||||
return models
|
||||
def meshTransformation():
|
||||
|
||||
|
||||
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
|
||||
if(aspDir == None):
|
||||
|
||||
# Коректировка пути до папки с генерацией ASP
|
||||
if (aspDir == None):
|
||||
args.print_helper()
|
||||
if(aspDir[aspDir.__len__() - 1] != '/'):
|
||||
if (aspDir[aspDir.__len__() - 1] != '/'):
|
||||
aspDir += '/'
|
||||
assemblys = FS.readFolder(aspDir + 'assembly')
|
||||
# Получение списка папок с .obj обьектами
|
||||
assembles = FS.readFolder(aspDir + 'assembly')
|
||||
assemblyDirNormalize = []
|
||||
for el in assemblys:
|
||||
for el in assembles:
|
||||
try:
|
||||
|
||||
process_mesh(source_dir=el, target_dir=el+'/process/',subdivide=el, verbose=True)
|
||||
# Пост обработка .obj обьектов
|
||||
process_mesh(source_dir=el, target_dir=el +
|
||||
'/process/', subdivide=el, verbose=True)
|
||||
assemblyDirNormalize.append(el + '/process/')
|
||||
except Exception as e:
|
||||
print('ERRROR:')
|
||||
print(e)
|
||||
|
||||
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)
|
||||
|
||||
|
||||
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]
|
||||
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]]})
|
||||
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' )
|
||||
|
||||
"status": status,
|
||||
}
|
||||
# Запись результата планирования
|
||||
FS.writeFile(json.dumps(planingObject),
|
||||
el[0:el.__len__() - 8], 'insertion_path.json')
|
||||
|
||||
try:
|
||||
planner = PyPlanner(assembly_dir, 'process', still_ids=[1], )
|
||||
planner = PyPlanner(assembly_dir, 'process', still_ids=[1],)
|
||||
status, t_plan, path = planner.plan(
|
||||
planner_name=args.planner,
|
||||
step_size=args.step_size,
|
||||
max_time=args.max_time,
|
||||
seed=args.seed,
|
||||
return_path=True,
|
||||
simplify=args.simplify,
|
||||
planner_name=args.planner,
|
||||
step_size=args.step_size,
|
||||
max_time=args.max_time,
|
||||
seed=args.seed,
|
||||
return_path=True,
|
||||
simplify=args.simplify,
|
||||
render=args.render
|
||||
)
|
||||
|
||||
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)
|
||||
planner.save_path(path, args.save_dir, args.n_save_state)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
|
||||
meshTransformation()
|
||||
|
||||
|
||||
main()
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue