diff --git a/cad_generation/scenarios/robossembler_freecad_export_scenario.py b/cad_generation/scenarios/robossembler_freecad_export_scenario.py index c834efb..717a127 100644 --- a/cad_generation/scenarios/robossembler_freecad_export_scenario.py +++ b/cad_generation/scenarios/robossembler_freecad_export_scenario.py @@ -1,7 +1,6 @@ from usecases.export_assembly_them_all_usecase import ExportAssemblyThemAllUseCase import FreeCAD - from usecases.export_usecase import EXPORT_TYPES, ExportUseCase from usecases.get_sdf_geometry_usecase import SdfGeometryUseCase from usecases.assembly_parse_usecase import AssemblyParseUseCase @@ -10,7 +9,6 @@ from model.geometry_part import GeometryPart from model.files_generator import FolderGenerator from helper.fs import FS import os -# import ImportGui import shutil diff --git a/cad_stability_check/main.py b/cad_stability_check/main.py index b4684e8..4ecc089 100644 --- a/cad_stability_check/main.py +++ b/cad_stability_check/main.py @@ -3,6 +3,7 @@ import json import re from pyquaternion import Quaternion + def importObjAtPath(path: str): import importOBJ importOBJ.insert(u"" + path, App.ActiveDocument.Label) @@ -14,7 +15,7 @@ def getFullPathObj(assemblyFolder: str, name: str): return assemblyFolder + 'sdf/meshes/' + name + '.obj' -def computedStabiliti(refElement, childElement): +def computedStability(refElement, childElement): b = childElement.Shape.BoundBox App.activeDocument().addObject("Part::MultiCommon", "Common") App.activeDocument().Common.Shapes = [refElement, childElement, ] @@ -34,16 +35,24 @@ def computedStabiliti(refElement, childElement): def main(): App.newDocument() + # загрузка env интерфейса env = json.loads((open('./env.json')).read()) - coordinatsFilePath = env.get('pathToTheSimulationCoordinatesFile') - assemblyFolder = env.get('generationFolder') - buildNumber = int(re.findall(r'\d', coordinatsFilePath)[0]) + # получение пути координат файла + coordinatesFilePath = env.get('pathToTheSimulationCoordinatesFile') + + # получение папки сборки + assemblyFolder = env.get('generationFolder') + buildNumber = int(re.findall(r'\d', coordinatesFilePath)[0]) + + # загрузка последовательности сборки ввиде списка строк деталей assemblyStructure = json.loads( (open(assemblyFolder + 'step-structure.json')).read()) + # получение номера тестируемой сборки assemblyNumber = int(buildNumber) + # получение тестируемой детали в сборке activeDetail = assemblyStructure[assemblyNumber] - + subassemblyNotParticipatingInMarkup = assemblyStructure[0:assemblyNumber - 1] detailOfTheMarkingZoneOfWhich = assemblyStructure[assemblyNumber - 1] importObjAtPath(getFullPathObj(assemblyFolder, activeDetail)) @@ -57,22 +66,24 @@ def main(): importObjAtPath(el) for el in App.ActiveDocument.Objects[2:App.ActiveDocument.Objects.__len__()]: el.ViewObject.ShapeColor = (0.32, 0.05, 0.38) - - coordinats = json.loads((open(coordinatsFilePath)).read()) + # загрузка координат + coordinates = json.loads((open(coordinatesFilePath)).read()) inc = 1 + # перемещение обьектов в документе на точки стабильность полученные из pybullet for el in App.ActiveDocument.Objects: - pos = coordinats[inc]['position'] - qua = coordinats[inc]['quaternion'] + pos = coordinates[inc]['position'] + qua = coordinates[inc]['quaternion'] new_quaternion = Quaternion(qua[0], qua[1], qua[2], qua[3]) rotation_matrix = new_quaternion.rotation_matrix current_position = el.Placement.Base - new_position = App.Vector(current_position.x, current_position.y, current_position.z) + new_position = App.Vector( + current_position.x, current_position.y, current_position.z) new_placement = App.Placement(new_position, rotation_matrix) el.Placement = new_placement App.ActiveDocument.recompute() el.Placement.move(App.Vector(pos[0], pos[1], pos[2])) - - print(computedStabiliti(meshMark, meshDetailOfTheMarkZone)) + # вычисление стабильность + computedStability(meshMark, meshDetailOfTheMarkZone) pass diff --git a/cad_stability_input/main.py b/cad_stability_input/main.py index 3b646fd..e0bb99d 100644 --- a/cad_stability_input/main.py +++ b/cad_stability_input/main.py @@ -20,7 +20,6 @@ def main(): App.newDocument() env = json.loads((open('./env.json')).read()) assemblyFolder = env.get('assemblyFolder') - assemblyStructure = json.loads( (open(assemblyFolder + 'step-structure.json')).read()) assemblyNumber = int(env.get('buildNumber')) - 1 diff --git a/geometric_feasibility_predicate/main.py b/geometric_feasibility_predicate/main.py index 8623d7c..32e592f 100644 --- a/geometric_feasibility_predicate/main.py +++ b/geometric_feasibility_predicate/main.py @@ -245,7 +245,7 @@ class GetPartPrimitiveCoordinatesUseCase(object): class InitPartsParseUseCase(): - + # Инициализация парсинга def call(self): product_details = [] for part in FreeCadRepository().getAllSolids(): @@ -257,13 +257,14 @@ class InitPartsParseUseCase(): class RenderPrimitiveUseCase(object): - + # Рендеринг премитивов def call(self, meshModels: list[MeshGeometryCoordinateModel], detailSquares) -> None: for mesh in meshModels: mesh.initializePrimitivesByCoordinate(detailSquares) class ClearWorkSpaceDocumentUseCase(object): + # Очистка рабочего простарнства def call(self, detailSquares): for key in detailSquares: for renderPrimitive in detailSquares[key]: @@ -309,7 +310,9 @@ class ClearWorkSpaceDocumentUseCase(object): class CadAdjacencyMatrix: + # Матрица основанная на соприкосновении примитива с обьектами def primitiveMatrix(self): + # Получение матрицы matrix = RenderPrimitivesScenario( InitPartsParseUseCase(), GetPartPrimitiveCoordinatesUseCase(), @@ -317,27 +320,27 @@ class CadAdjacencyMatrix: GetCollisionAtPrimitiveUseCase(), ClearWorkSpaceDocumentUseCase(), ).call() - return AdjacencyMatrix( all_parts=GetAllPartsLabelsUseCase().call(), first_detail=GetFirstDetailUseCase().call(), matrix=matrix, ) - - def matrixBySurfaces(self,): - adjaxed = {} + # Матрица основанная на соприкосновении обьектов + def matrixBySurfaces(self): + matrix = {} for part in FreeCadRepository().getAllSolids(): - adjaxed[part.Label] = [] + matrix[part.Label] = [] for nextPart in FreeCadRepository().getAllSolids(): if part.Label != nextPart.Label: + # Вычисление соприконсоновения площади деталей collisionResult: int = int( part.Shape.distToShape(nextPart.Shape)[0]) if (collisionResult == 0): - adjaxed[part.Label].append(nextPart.Label) + matrix[part.Label].append(nextPart.Label) return AdjacencyMatrix(all_parts=GetAllPartsLabelsUseCase( ).call(), first_detail=GetFirstDetailUseCase().call(), - matrix=adjaxed + matrix=matrix ) @@ -349,9 +352,10 @@ def main(): return TypeError('CadFile not found env.json') App.open(u'' + cadFile) - + # Получение матрицы matrixOut = CadAdjacencyMatrix().primitiveMatrix().to_dict() import json + # Запись результата FS.writeFile(json.dumps(matrixOut, ensure_ascii=False, indent=4), outPath,'out.json') diff --git a/insertion_vector_predicate/main.py b/insertion_vector_predicate/main.py index ebdf9f7..84f13b7 100644 --- a/insertion_vector_predicate/main.py +++ b/insertion_vector_predicate/main.py @@ -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() \ No newline at end of file + + +main() diff --git a/stability_process_predicate/main.py b/stability_process_predicate/main.py index a4e0304..3f027c9 100644 --- a/stability_process_predicate/main.py +++ b/stability_process_predicate/main.py @@ -2,15 +2,15 @@ import argparse from usecases.stability_check_usecase import StabilityCheckUseCase -#python3 main.py --aspPath /home/idontsudo/t/framework/asp/out/sdf-generation --buildNumber 3 +# python3 main.py --aspPath /home/idontsudo/t/framework/asp/out/sdf-generation --buildNumber 3 + + def main(): parser = argparse.ArgumentParser() parser.add_argument('--aspPath', help='asp folder generation path') parser.add_argument('--buildNumber', help='FreeCad generation buildNumber') args = parser.parse_args() - # args.aspPath - # args.buildNumber - StabilityCheckUseCase().call( args.aspPath,args.buildNumber ) + StabilityCheckUseCase().call(args.aspPath, args.buildNumber) -main() \ No newline at end of file +main() diff --git a/stability_process_predicate/usecases/stability_check_usecase.py b/stability_process_predicate/usecases/stability_check_usecase.py index 9d37b53..561f885 100644 --- a/stability_process_predicate/usecases/stability_check_usecase.py +++ b/stability_process_predicate/usecases/stability_check_usecase.py @@ -10,6 +10,7 @@ class StabilityCheckUseCase: def call(self, outPath: str, buildNumber: int, duration=500): DURATION = duration try: + # Получение сгенерированного URDF для сборки assemblyUrdf = json.loads( (open(outPath + 'urdf-generation.json')).read()).get(buildNumber) except: @@ -41,6 +42,7 @@ class StabilityCheckUseCase: inc = 0 for el in bulletIds: inc += 1 + # Получение расположения деталей pos, rot = p.getBasePositionAndOrientation(el) resultCoords.append({ 'id': inc, @@ -52,8 +54,8 @@ class StabilityCheckUseCase: file_to_open = outPath + buildNumber + "_" + 'stability_coords.json' - f = open(file_to_open, 'w', encoding='utf-8', - errors='ignore') + f = open(file_to_open, 'w', encoding='utf-8',errors='ignore') + # Запись результатов тестирования f.write(json.dumps(resultCoords)) for el in urdfs: os.remove(el)