diff --git a/asp/helper/fs.py b/asp/helper/fs.py index 0cdfb23..4031795 100644 --- a/asp/helper/fs.py +++ b/asp/helper/fs.py @@ -8,18 +8,21 @@ class FS: return json.loads((open(path)).read()) def writeFile(data, filePath, fileName): - file_to_open = filePath + fileName - f = open(file_to_open, 'w', ) - + f = open(file_to_open, "w", encoding="utf-8", errors="ignore") f.write(data) - def readFile(path:str): + f.close() + + def readFile(path: str): return open(path).read() - - def readFilesTypeFolder(pathFolder: str, fileType = '.json'): + + def readFilesTypeFolder(pathFolder: str, fileType=".json"): filesJson = list( - filter(lambda x: x[-fileType.__len__():] == fileType, os.listdir(pathFolder))) + filter( + lambda x: x[-fileType.__len__() :] == fileType, os.listdir(pathFolder) + ) + ) return filesJson @@ -30,6 +33,5 @@ def listGetFirstValue(iterable, default=False, pred=None): def filterModels(filterModels, filterModelsDescription: list[str]): models = [] for el in filterModelsDescription: - models.append(listGetFirstValue( - filterModels, None, lambda x: x.name == el)) + models.append(listGetFirstValue(filterModels, None, lambda x: x.name == el)) return models diff --git a/asp/main.py b/asp/main.py index 17783c9..7ed3455 100644 --- a/asp/main.py +++ b/asp/main.py @@ -1,45 +1,53 @@ import argparse import shutil +from src.model.enum import Enum from helper.fs import FS from src.usecases.urdf_sub_assembly_usecase import UrdfSubAssemblyUseCase from src.model.sdf_geometry import GeometryModel from src.usecases.sdf_sub_assembly_usecase import SdfSubAssemblyUseCase import os +from pathlib import Path + if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument('--generationFolder', help='FreeCad generation folder') - parser.add_argument('--outPath', help='save SDF path') - parser.add_argument('--world', help='adding sdf world') - parser.add_argument('--format', help='urdf,sdf,mujoco') + parser.add_argument("--generationFolder", help="FreeCad generation folder") + parser.add_argument("--outPath", help="save SDF path") + parser.add_argument("--world", help="adding sdf world") + parser.add_argument("--format", help="urdf,sdf,mujoco") args = parser.parse_args() if args.generationFolder == None or args.outPath == None: parser.print_help() outPath = args.outPath - geometryFiles = FS.readFilesTypeFolder(args.generationFolder + '/assets/') - assemblyStructure = FS.readJSON( - args.generationFolder + '/step-structure.json') + geometryFiles = FS.readFilesTypeFolder(args.generationFolder + "/assets/") + assemblyStructure = FS.readJSON(args.generationFolder + "/step-structure.json") geometryModels: list[GeometryModel] = [] for el in geometryFiles: - geometryModels.append(GeometryModel.from_dict( - FS.readJSON(args.generationFolder + '/assets/' + el))) - # if os.path.exists(outPath + 'sdf-generation/'): - # shutil.rmtree(path=outPath + 'sdf-generation/') + geometryModels.append( + GeometryModel.from_dict( + FS.readJSON(args.generationFolder + "/assets/" + el) + ) + ) + if os.path.exists(outPath + Enum.folderPath): + shutil.rmtree(outPath + Enum.folderPath) + Path(outPath + Enum.folderPath).mkdir(parents=True, exist_ok=True) - if (args.format == 'sdf'): + if args.format == "sdf": SdfSubAssemblyUseCase().call( - geometryModels=geometryModels, assembly=assemblyStructure, + geometryModels=geometryModels, + assembly=assemblyStructure, world=args.world, generationFolder=args.generationFolder, - outPath=args.outPath + outPath=args.outPath, ) - if (args.format == 'urdf'): + if args.format == "urdf": UrdfSubAssemblyUseCase().call( - geometryModels=geometryModels, assembly=assemblyStructure, + geometryModels=geometryModels, + assembly=assemblyStructure, world=args.world, generationFolder=args.generationFolder, - outPath=args.outPath + outPath=args.outPath, ) diff --git a/asp/mocks/sdf/world.sdf b/asp/mocks/sdf/world.sdf index 4014b10..bd583fa 100644 --- a/asp/mocks/sdf/world.sdf +++ b/asp/mocks/sdf/world.sdf @@ -1,14 +1,7 @@ - - 0.001 - 1 - 1000 - - - - - + + 0 0 -9.8 6e-06 2.3e-05 -4.2e-05 @@ -66,6 +59,6 @@ 0 0 0 0 -0 0 false - + diff --git a/asp/src/model/enum.py b/asp/src/model/enum.py index 88f24f8..9e2c5ef 100644 --- a/asp/src/model/enum.py +++ b/asp/src/model/enum.py @@ -1,2 +1,2 @@ class Enum: - folderPath = 'sdf-generation/'; + folderPath = "generation/" diff --git a/asp/src/model/sdf_geometry.py b/asp/src/model/sdf_geometry.py index 3fdaa8b..5118152 100644 --- a/asp/src/model/sdf_geometry.py +++ b/asp/src/model/sdf_geometry.py @@ -34,7 +34,29 @@ DELIMITER_SCALE = 10000 class GeometryModel: - def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction, centerMassX, centerMassY, centerMassZ): + def __init__( + self, + name, + ixx, + ixy, + ixz, + iyy, + izz, + massSDF, + posX, + posY, + posZ, + eulerX, + eulerY, + eulerZ, + iyz, + stl, + link, + friction, + centerMassX, + centerMassY, + centerMassZ, + ): self.name = name self.ixx = ixx self.ixy = ixy @@ -74,12 +96,33 @@ class GeometryModel: eulerZ = from_union([from_str, from_none], obj.get("eulerZ")) iyz = from_union([from_str, from_none], obj.get("iyz")) stl = from_union([from_str, from_none], obj.get("stl")) - link = from_union([from_str, from_none], obj.get('link')) + link = from_union([from_str, from_none], obj.get("link")) friction = from_union([from_str, from_none], obj.get("friction")) centerMassX = from_union([from_str, from_none], obj.get("centerMassX")) centerMassY = from_union([from_str, from_none], obj.get("centerMassY")) centerMassZ = from_union([from_str, from_none], obj.get("centerMassZ")) - return GeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction, centerMassX, centerMassY, centerMassZ) + return GeometryModel( + name, + ixx, + ixy, + ixz, + iyy, + izz, + massSDF, + posX, + posY, + posZ, + eulerX, + eulerY, + eulerZ, + iyz, + stl, + link, + friction, + centerMassX, + centerMassY, + centerMassZ, + ) def to_dict(self): result = {} @@ -114,40 +157,112 @@ class GeometryModel: if self.stl is not None: result["stl"] = from_union([from_str, from_none], self.stl) if self.link is not None: - result['link'] = from_union([from_str, from_none], self.link) + result["link"] = from_union([from_str, from_none], self.link) if self.friction is not None: result["friction"] = from_union([from_str, from_none], self.eulerZ) if self.centerMassX is not None: - result['centerMassX'] = from_union( - [from_str, from_none], self.centerMassX) + result["centerMassX"] = from_union([from_str, from_none], self.centerMassX) if self.centerMassY is not None: - result['centerMassY'] = from_union( - [from_str, from_none], self.centerMassY) + result["centerMassY"] = from_union([from_str, from_none], self.centerMassY) if self.centerMassZ is not None: - result['centerMassZ'] = from_union( - [from_str, from_none], self.centerMassZ) + result["centerMassZ"] = from_union([from_str, from_none], self.centerMassZ) return result def toJSON(self) -> str: - return str(self.to_dict()).replace('\'', '"') + return str(self.to_dict()).replace("'", '"') def toSDF(self): - return FS.readFile(os.path.dirname(os.path.realpath(__file__)) - + '/../../mocks/sdf/model.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}', self.friction) + return ( + FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../../mocks/sdf/model.sdf" + ) + .replace( + "{name}", + self.name, + ) + .replace("{posX}", self.posX) + .replace("{posY}", self.posY) + .replace("{posZ}", self.posZ) + .replace("{eulerX}", self.eulerX) + .replace("{eulerY}", self.eulerY) + .replace("{eulerZ}", self.eulerZ) + .replace("{ixx}", self.ixx) + .replace("{ixy}", self.ixy) + .replace("{ixz}", self.ixz) + .replace("{iyy}", self.iyy) + .replace("{iyz}", self.iyz) + .replace("{izz}", self.izz) + .replace( + "{massSDF}", + self.massSDF, + ) + .replace("{stl}", self.stl) + .replace("{friction}", self.friction) + ) def toSdfLink(self): - - return FS.readFile(os.path.dirname(os.path.realpath(__file__)) - + '/../../mocks/sdf/link.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}', self.friction) + return ( + FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../../mocks/sdf/link.sdf" + ) + .replace( + "{name}", + self.name, + ) + .replace("{posX}", self.posX) + .replace("{posY}", self.posY) + .replace("{posZ}", self.posZ) + .replace("{eulerX}", self.eulerX) + .replace("{eulerY}", self.eulerY) + .replace("{eulerZ}", self.eulerZ) + .replace("{ixx}", self.ixx) + .replace("{ixy}", self.ixy) + .replace("{ixz}", self.ixz) + .replace("{iyy}", self.iyy) + .replace("{iyz}", self.iyz) + .replace("{izz}", self.izz) + .replace( + "{massSDF}", + self.massSDF, + ) + .replace("{stl}", self.stl) + .replace("{friction}", self.friction) + ) def includeLink(self, pose=False): - if (pose == False): - return FS.readFile(os.path.dirname(os.path.realpath(__file__)) - + '/../../mocks/sdf/include.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name) - return FS.readFile(os.path.dirname(os.path.realpath(__file__)) - + '/../../mocks/sdf/include_pose.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz) + if pose == False: + return ( + FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../../mocks/sdf/include.sdf" + ) + .replace("{name}", self.name) + .replace("{uri}", "/" + self.name) + ) + return ( + FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../../mocks/sdf/include_pose.sdf" + ) + .replace("{name}", self.name) + .replace("{uri}", "/" + self.name) + .replace("{posX}", self.posX) + .replace("{posY}", self.posY) + .replace("{posZ}", self.posZ) + .replace("{eulerX}", self.eulerX) + .replace("{eulerY}", self.eulerY) + .replace("{eulerZ}", self.eulerZ) + .replace("{ixx}", self.ixx) + .replace("{ixy}", self.ixy) + .replace("{ixz}", self.ixz) + .replace("{iyy}", self.iyy) + .replace("{iyz}", self.iyz) + .replace("{izz}", self.izz) + ) - def generateSDFatJoinFixed(self, sdfModels: list['GeometryModel']): + def generateSDFatJoinFixed(self, sdfModels: list["GeometryModel"]): sdf = '\n\n' sdf += ' \n' sdf += " 0 0 0 0 0 0\n" @@ -157,25 +272,56 @@ class GeometryModel: if sdfModels.__len__() == 0: return link endTagLinkInc = link.__len__() - beginSDF = link[0: endTagLinkInc] + beginSDF = link[0:endTagLinkInc] - sdfJoin = beginSDF + '\n' + sdfJoin = beginSDF + "\n" for el in sdfModels: if el.name != self.name: - sdfJoin += el.includeLink(pose=True) + '\n' + sdfJoin += el.includeLink(pose=True) + "\n" - endSDF = link[endTagLinkInc:link.__len__()] + endSDF = link[endTagLinkInc : link.__len__()] for el in sdfModels: if el.name != self.name: - sdfJoin += SdfJoin(name=str(uuid.uuid4()), - parent=self.name, child=el.name, modelAt=el).toSDF() + '\n' + sdfJoin += ( + SdfJoin( + name=str(uuid.uuid4()), + parent=self.name, + child=el.name, + modelAt=el, + ).toSDF() + + "\n" + ) sdfJoin += endSDF - sdfJoin += '' + sdfJoin += "" return sdfJoin def toUrdf(self): - return FS.readFile(os.path.dirname(os.path.realpath(__file__)) - + '/../../mocks/urdf/model.urdf').replace('{name}', self.name).replace('{name}', self.name).replace('{uri}', '/' + self.name).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{stl}', '/' + self.stl).replace('{massSDF}', self.massSDF).replace('{centerMassX}', self.centerMassX).replace('{centerMassY}', self.centerMassY).replace('{centerMassZ}', self.centerMassZ) + return ( + FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../../mocks/urdf/model.urdf" + ) + .replace("{name}", self.name) + .replace("{name}", self.name) + .replace("{uri}", "/" + self.name) + .replace("{posX}", self.posX) + .replace("{posY}", self.posY) + .replace("{posZ}", self.posZ) + .replace("{eulerX}", self.eulerX) + .replace("{eulerY}", self.eulerY) + .replace("{eulerZ}", self.eulerZ) + .replace("{ixx}", self.ixx) + .replace("{ixy}", self.ixy) + .replace("{ixz}", self.ixz) + .replace("{iyy}", self.iyy) + .replace("{iyz}", self.iyz) + .replace("{izz}", self.izz) + .replace("{stl}", self.stl) + .replace("{massSDF}", self.massSDF) + .replace("{centerMassX}", self.centerMassX) + .replace("{centerMassY}", self.centerMassY) + .replace("{centerMassZ}", self.centerMassZ) + ) diff --git a/asp/src/usecases/generate_world.py b/asp/src/usecases/generate_world.py index 96b842e..bfe9681 100644 --- a/asp/src/usecases/generate_world.py +++ b/asp/src/usecases/generate_world.py @@ -1,12 +1,18 @@ import os from helper.fs import FS + class SdfGenerateWorldUseCase: - def call(assembly:str) -> str: - world = FS.readFile(os.path.dirname(os.path.realpath(__file__)) - + '/../../mocks/sdf/world.sdf') - beginWorld = world[0:world.find(' str: + world = FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + "/../../mocks/sdf/world.sdf" + ) + beginWorld = world[0 : world.find(" str: + return diff --git a/asp/src/usecases/sdf_sub_assembly_usecase.py b/asp/src/usecases/sdf_sub_assembly_usecase.py index 12be136..7f36bb3 100644 --- a/asp/src/usecases/sdf_sub_assembly_usecase.py +++ b/asp/src/usecases/sdf_sub_assembly_usecase.py @@ -9,49 +9,70 @@ from src.usecases.sdf_generate_world_usecase import SdfGenerateWorldUseCase from src.model.sdf_geometry import GeometryModel from distutils.dir_util import copy_tree -SDF_FILE_FORMAT = '.sdf' -CONFIG_PATH = os.path.dirname(os.path.realpath( - __file__)) + '/../../mocks/sdf/model.config' - - +SDF_FILE_FORMAT = ".sdf" +CONFIG_PATH = ( + os.path.dirname(os.path.realpath(__file__)) + "/../../mocks/sdf/model.config" +) class SdfSubAssemblyUseCase(Assembly): - - def call(self, geometryModels: list[GeometryModel], assembly: list[str], outPath: str, generationFolder: str, world: bool): + def call( + self, + geometryModels: list[GeometryModel], + assembly: list[str], + outPath: str, + generationFolder: str, + world: bool, + ): asm = {} generateSubAssemblyModels = self.generateSubAssembly(assembly) inc = 0 for key, value in generateSubAssemblyModels.items(): inc += 1 - if value['assembly'].__len__() != 0: - + if value["assembly"].__len__() != 0: model: Optional[GeometryModel] = listGetFirstValue( - geometryModels, None, lambda x: x.name == value['assembly'][0]) + geometryModels, None, lambda x: x.name == value["assembly"][0] + ) if model != None: + asm[key] = { + "assembly": model.generateSDFatJoinFixed( + filterModels(geometryModels, value["assembly"]) + ), + "part": ( + listGetFirstValue( + geometryModels, None, lambda x: x.name == value["part"] + ) + ).includeLink(), + } - asm[key] = {"assembly": model.generateSDFatJoinFixed(filterModels(geometryModels, value['assembly'])), "part": ( - listGetFirstValue(geometryModels, None, lambda x: x.name == value['part'])).includeLink()} - - self.copy(generationFolder= - generationFolder, format='/sdf', outPath=outPath) + self.copy(generationFolder=generationFolder, format="/sdf", outPath=outPath) dirPath = outPath + Enum.folderPath for el in geometryModels: - path = dirPath + el.name + '/' + path = dirPath + el.name + "/" os.makedirs(path) - FS.writeFile(data=el.toSDF(), filePath=path, - fileName='/model' + SDF_FILE_FORMAT) - FS.writeFile(data=FS.readFile(CONFIG_PATH), - filePath=path, fileName='/model' + '.config') + FS.writeFile( + data=el.toSDF(), filePath=path, fileName="/model" + SDF_FILE_FORMAT + ) + FS.writeFile( + data=FS.readFile(CONFIG_PATH), + filePath=path, + fileName="/model" + ".config", + ) for key, v in asm.items(): - FS.writeFile(data=v['assembly'], filePath=dirPath, - fileName='/' + key + SDF_FILE_FORMAT) - + FS.writeFile( + data=v["assembly"], + filePath=dirPath, + fileName="/" + key + SDF_FILE_FORMAT, + ) + else: for key, v in asm.items(): - FS.writeFile(data=SdfGenerateWorldUseCase.call(v['assembly']), filePath=dirPath, - fileName='/' + key + SDF_FILE_FORMAT) + FS.writeFile( + data=SdfGenerateWorldUseCase.call(v["assembly"]), + filePath=dirPath, + fileName="/" + key + SDF_FILE_FORMAT, + ) FormatterUseCase.call(outPath=outPath, format=SDF_FILE_FORMAT) diff --git a/asp/src/usecases/urdf_sub_assembly_usecase.py b/asp/src/usecases/urdf_sub_assembly_usecase.py index 4d652f7..461fa9e 100644 --- a/asp/src/usecases/urdf_sub_assembly_usecase.py +++ b/asp/src/usecases/urdf_sub_assembly_usecase.py @@ -6,14 +6,25 @@ import json import re -URDF_FILE_FORMAT = '.urdf' -URDF_GENERATOR_FILE = 'urdf-generation' + '.json' +URDF_FILE_FORMAT = ".urdf" +URDF_GENERATOR_FILE = "urdf-generation" + ".json" + class UrdfSubAssemblyUseCase(Assembly): - def call(self, geometryModels: list[GeometryModel], assembly: list[str], outPath: str, generationFolder: str, world: bool): + def call( + self, + geometryModels: list[GeometryModel], + assembly: list[str], + outPath: str, + generationFolder: str, + world: bool, + ): dirPath = generationFolder + Enum.folderPath asm = {} for el in geometryModels: asm[el.name] = el.toUrdf() - FS.writeFile(data=json.dumps(asm,indent=4), - fileName=URDF_GENERATOR_FILE, filePath=dirPath) + FS.writeFile( + data=json.dumps(asm, indent=4), + fileName=URDF_GENERATOR_FILE, + filePath=dirPath, + ) diff --git a/cad_generation/env.json b/cad_generation/env.json index f9633a4..f0f5724 100644 --- a/cad_generation/env.json +++ b/cad_generation/env.json @@ -1,6 +1,4 @@ { - "doc": "/home/idontsudo/framework/asp/out/disk_and_axis_n.FCStd", - "out": "/home/idontsudo/framework/asp/out", - "resultURL": "http://localhost:3002/assembly/save/out", - "projectId": "cubes" -} \ No newline at end of file + "cadFilePath": "/Users/idontsudo/Desktop/asp-example/disk_and_axis_n.FCStd", + "outPath": "/Users/idontsudo/Desktop/asp-example/" +} diff --git a/cad_generation/helper/file_system_repository.py b/cad_generation/helper/file_system_repository.py new file mode 100644 index 0000000..a2419d8 --- /dev/null +++ b/cad_generation/helper/file_system_repository.py @@ -0,0 +1,28 @@ +import os +import json +import shutil + + +class FileSystemRepository: + def readJSON(path: str): + return json.loads((open(path)).read()) + + def recursiveDeleteFolder(path: str): + shutil.rmtree(path) + pass + + def deletingOldAndCreatingNewFolder(path: str): + if FileSystemRepository.isExistsPath(path): + FileSystemRepository.recursiveDeleteFolder(path) + os.makedirs(path) + pass + + def isExistsPath(path: str): + return os.path.exists(path) + + def writeFile(data, filePath, fileName): + file_to_open = filePath + fileName + + f = open(file_to_open, "w", encoding="utf-8", errors="ignore") + f.write(data) + f.close() diff --git a/cad_generation/helper/fs.py b/cad_generation/helper/fs.py deleted file mode 100644 index 3026bb4..0000000 --- a/cad_generation/helper/fs.py +++ /dev/null @@ -1,15 +0,0 @@ -import os -import json - - -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', encoding='utf-8', - errors='ignore') - f.write(data) - f.close() diff --git a/cad_generation/main.py b/cad_generation/main.py index 83ddba7..24eabf7 100644 --- a/cad_generation/main.py +++ b/cad_generation/main.py @@ -1,19 +1,25 @@ -import requests import FreeCAD as App -from helper.fs import FS -from scenarios.robossembler_freecad_export_scenario import RobossemblerFreeCadExportScenario -import shutil -import os +from helper.file_system_repository import FileSystemRepository +from scenarios.robossembler_freecad_export_scenari import ( + RobossemblerFreeCadExportScenari, +) + + import FreeCADGui as Gui +# obj.Support[0][0].Label +# 'Hex_King' + +import FreeCAD as App + def main(): - env = FS.readJSON('./env.json') - App.openDocument(env.get('doc')) - RobossemblerFreeCadExportScenario().call(env.get('out')) - # requests.post(url=env.get('resultURL'), files={'zip': open(env.get('out') + '/' + 'generation.zip', "rb"), 'id':env.get('projectId')}) - # os.remove('./generation.zip') + env = FileSystemRepository.readJSON("./env.json") + App.openDocument(env.get("cadFilePath")) + RobossemblerFreeCadExportScenari.call(env.get("outPath")) App.closeDocument(App.ActiveDocument.Name) - freecadQTWindow = Gui.getMainWindow() + freecadQTWindow = Gui.getMainWindow() freecadQTWindow.close() + + main() diff --git a/cad_generation/model/sdf_geometry_model.py b/cad_generation/model/sdf_geometry_model.py index 21c2a4e..c08e0d8 100644 --- a/cad_generation/model/sdf_geometry_model.py +++ b/cad_generation/model/sdf_geometry_model.py @@ -26,7 +26,28 @@ def to_class(c, x): class SdfGeometryModel: - def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction, centerMassX, centerMassY, centerMassZ,): + def __init__( + self, + name, + ixx, + ixy, + ixz, + iyy, + izz, + massSDF, + posX, + posY, + posZ, + eulerX, + eulerY, + eulerZ, + iyz, + stl, + friction, + centerMassX, + centerMassY, + centerMassZ, + ): self.name = name self.ixx = ixx self.ixy = ixy @@ -69,7 +90,27 @@ class SdfGeometryModel: centerMassX = from_union([from_str, from_none], obj.get("centerMassX")) centerMassY = from_union([from_str, from_none], obj.get("centerMassY")) centerMassZ = from_union([from_str, from_none], obj.get("centerMassZ")) - return SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction, centerMassX, centerMassY, centerMassZ) + return SdfGeometryModel( + name, + ixx, + ixy, + ixz, + iyy, + izz, + massSDF, + posX, + posY, + posZ, + eulerX, + eulerY, + eulerZ, + iyz, + stl, + friction, + centerMassX, + centerMassY, + centerMassZ, + ) def to_dict(self): result = {} @@ -105,14 +146,13 @@ class SdfGeometryModel: result["stl"] = from_union([from_str, from_none], self.stl) if self.friction is not None: result["friction"] = from_union([from_str, from_none], self.eulerZ) - if self.centerMassX is not None: - result['centerMassX'] = from_union([from_str, from_none], self.centerMassX) + result["centerMassX"] = from_union([from_str, from_none], self.centerMassX) if self.centerMassY is not None: - result['centerMassY'] = from_union([from_str, from_none], self.centerMassY) + result["centerMassY"] = from_union([from_str, from_none], self.centerMassY) if self.centerMassZ is not None: - result['centerMassZ'] = from_union([from_str, from_none], self.centerMassZ) + result["centerMassZ"] = from_union([from_str, from_none], self.centerMassZ) return result def toJSON(self) -> str: - return str(self.to_dict()).replace('\'', '"') + return str(self.to_dict()).replace("'", '"') diff --git a/cad_generation/scenarios/robossembler_freecad_export_scenari.py b/cad_generation/scenarios/robossembler_freecad_export_scenari.py new file mode 100644 index 0000000..75c27da --- /dev/null +++ b/cad_generation/scenarios/robossembler_freecad_export_scenari.py @@ -0,0 +1,49 @@ +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 +from model.files_generator import FolderGenerator +from helper.file_system_repository import FileSystemRepository +import os + + +class RobossemblerFreeCadExportScenari: + def call(path): + directory = path + + __objs__ = FreeCAD.ActiveDocument.RootObjects + directoryExport = directory + "/" + FileSystemRepository.deletingOldAndCreatingNewFolder( + directoryExport + FolderGenerator.ASSETS.value + ) + FileSystemRepository.deletingOldAndCreatingNewFolder( + directoryExport + FolderGenerator.SDF.value, + ) + + FileSystemRepository.deletingOldAndCreatingNewFolder( + directoryExport + + FolderGenerator.SDF.value + + "/" + + FolderGenerator.MESHES.value + ) + FileSystemRepository.deletingOldAndCreatingNewFolder( + directoryExport + FolderGenerator.ASSEMBlY.value + ) + f = open(directory + "/step-structure.json", "w") + f.write(AssemblyParseUseCase().toJson()) + f.close() + RobossemblerFreeCadExportScenari.geometry(directory) + ExportAssemblyThemAllUseCase().call(directoryExport) + + return True + + def geometry(outPutsPath: str): + exportUseCase = ExportUseCase.call(outPutsPath, EXPORT_TYPES.OBJ) + + for el in SdfGeometryUseCase().call(exportUseCase): + FileSystemRepository.writeFile( + el.toJSON(), + outPutsPath + FolderGenerator.ASSETS.value + "/", + el.name + ".json", + ) diff --git a/cad_generation/scenarios/robossembler_freecad_export_scenario.py b/cad_generation/scenarios/robossembler_freecad_export_scenario.py deleted file mode 100644 index c5b74da..0000000 --- a/cad_generation/scenarios/robossembler_freecad_export_scenario.py +++ /dev/null @@ -1,52 +0,0 @@ - -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 -from usecases.geometry_usecase import GeometryUseCase -from model.geometry_part import GeometryPart -from model.files_generator import FolderGenerator -from helper.fs import FS -import os -import shutil - - -class RobossemblerFreeCadExportScenario: - - def call(self, path): - - - directory = path + '/' + 'generation' - if os.path.exists(directory): - shutil.rmtree(directory) - if not os.path.exists(directory): - os.makedirs(directory) - - __objs__ = FreeCAD.ActiveDocument.RootObjects - directoryExport = directory + '/' - os.makedirs(directoryExport + FolderGenerator.ASSETS.value) - - os.makedirs(directoryExport + FolderGenerator.SDF.value) - os.makedirs(directoryExport + FolderGenerator.SDF.value + '/' + FolderGenerator.MESHES.value) - os.makedirs(directoryExport + FolderGenerator.ASSEMBlY.value) - f = open(directory + "/step-structure.json", "w") - f.write(AssemblyParseUseCase().toJson()) - f.close() - self.geometry(directory) - ExportAssemblyThemAllUseCase().call(directoryExport) - - # shutil.make_archive(directory, 'zip', directory) - - # shutil.rmtree(directory) - return True - - def geometry(self, outPutsPath: str): - exportUseCase = ExportUseCase.call(outPutsPath,EXPORT_TYPES.OBJ) - for el in SdfGeometryUseCase().call(exportUseCase): - FS.writeFile(el.toJSON(), outPutsPath + '/' + FolderGenerator.ASSETS.value + '/', el.name + '.json',) - - - - - \ No newline at end of file diff --git a/cad_generation/usecases/export_assembly_them_all_usecase.py b/cad_generation/usecases/export_assembly_them_all_usecase.py index b8f0a57..837059a 100644 --- a/cad_generation/usecases/export_assembly_them_all_usecase.py +++ b/cad_generation/usecases/export_assembly_them_all_usecase.py @@ -1,11 +1,9 @@ - - from typing import List import FreeCAD as App import Part from model.join_mesh_model import JoinMeshModel from model.mesh_part_model import MeshPartModel -from helper.fs import FS +from helper.file_system_repository import FileSystemRepository from helper.is_solid import is_object_solid from model.simple_copy_part_model import SimpleCopyPartModel from model.files_generator import FolderGenerator @@ -15,78 +13,106 @@ import json class ExportAssemblyThemAllUseCase: - def call(self, path): assembly = AssemblyParseUseCase().getAsm() asmStructure = {} inc = 0 for el in assembly: - if (inc != 0): - asmStructure[inc] = { - "child": el, - "parents": assembly[0:inc] - } + if inc != 0: + asmStructure[inc] = {"child": el, "parents": assembly[0:inc]} inc += 1 objectsFreeCad = App.ActiveDocument.Objects asmSolids = {} for k, v in asmStructure.items(): - assemblyParentList = v['parents'] - assemblyChild = v['child'] + assemblyParentList = v["parents"] + assemblyChild = v["child"] for el in assemblyParentList: for solid in objectsFreeCad: - if (el == solid.Label): - if (asmSolids.get(k) is None): + if el == solid.Label: + if asmSolids.get(k) is None: + asmSolids[k] = { + "parents": [], + "child": list( + filter( + lambda x: x.Label == assemblyChild, + objectsFreeCad, + ) + )[0], + } - asmSolids[k] = {'parents': [], 'child': list( - filter(lambda x: x.Label == assemblyChild, objectsFreeCad))[0]} - - asmSolids[k]['parents'].append(solid) + asmSolids[k]["parents"].append(solid) inc = 0 for k, v in asmSolids.items(): geometry = {"0": [], "1": []} - if (k != 0): + if k != 0: App.activeDocument().addObject("Part::Compound", "Compound") - copyLinks = list( - map(lambda el: SimpleCopyPartModel(el), v['parents'])) + copyLinks = list(map(lambda el: SimpleCopyPartModel(el), v["parents"])) if copyLinks != None: App.activeDocument().Compound.Links = list( - map(lambda el: el.getPart(), copyLinks)) + map(lambda el: el.getPart(), copyLinks) + ) - object = App.activeDocument().getObject('Compound') + object = App.activeDocument().getObject("Compound") boundBox = object.Shape.BoundBox - geometry['0'].append(boundBox.XMax) - geometry['0'].append(boundBox.YMax) - geometry['0'].append(boundBox.ZMax) + geometry["0"].append(boundBox.XMax) + geometry["0"].append(boundBox.YMax) + geometry["0"].append(boundBox.ZMax) os.makedirs( - path + FolderGenerator.ASSEMBlY.value + '/' + '0000' + str(k)) - boundBoxChild = v['child'].Shape.BoundBox - geometry['1'].append(boundBoxChild.XMax) - geometry['1'].append(boundBoxChild.YMax) - geometry['1'].append(boundBoxChild.ZMax) + path + FolderGenerator.ASSEMBlY.value + "/" + "0000" + str(k) + ) + boundBoxChild = v["child"].Shape.BoundBox + geometry["1"].append(boundBoxChild.XMax) + geometry["1"].append(boundBoxChild.YMax) + geometry["1"].append(boundBoxChild.ZMax) meshParents = [] - - for el in v['parents']: + + for el in v["parents"]: meshParents.append(MeshPartModel(el)) joinMesh = JoinMeshModel(meshParents) for el in meshParents: el.remove() import importOBJ - importOBJ.export(joinMesh.mesh, path + FolderGenerator.ASSEMBlY.value + - '/' + '0000' + str(k) + '/' + str(1) + '.obj') + + importOBJ.export( + joinMesh.mesh, + path + + FolderGenerator.ASSEMBlY.value + + "/" + + "0000" + + str(k) + + "/" + + str(1) + + ".obj", + ) joinMesh.remove() - importOBJ.export(v['child'], path + FolderGenerator.ASSEMBlY.value + - '/' + '0000' + str(k) + '/' + str(0) + '.obj') - FS.writeFile(json.dumps(geometry), path + FolderGenerator.ASSEMBlY.value + - '/' + '0000' + str(k) + '/', 'translation.json') + importOBJ.export( + v["child"], + path + + FolderGenerator.ASSEMBlY.value + + "/" + + "0000" + + str(k) + + "/" + + str(0) + + ".obj", + ) + FileSystemRepository.writeFile( + json.dumps(geometry), + path + + FolderGenerator.ASSEMBlY.value + + "/" + + "0000" + + str(k) + + "/", + "translation.json", + ) App.ActiveDocument.removeObject("Compound") for el in copyLinks: el.remove() App.activeDocument().recompute() inc += 1 - - diff --git a/cad_generation/usecases/export_usecase.py b/cad_generation/usecases/export_usecase.py index fed67fa..6416821 100644 --- a/cad_generation/usecases/export_usecase.py +++ b/cad_generation/usecases/export_usecase.py @@ -4,33 +4,54 @@ import FreeCAD as App from model.files_generator import FolderGenerator from helper.is_solid import is_object_solid from enum import Enum - + + class EXPORT_TYPES(Enum): - STL = 'STL' - DAO = 'DAO' - OBJ = 'OBJ' - + STL = "STL" + DAO = "DAO" + OBJ = "OBJ" + class ExportUseCase: def call(path: str, type: EXPORT_TYPES): meshes = {} for el in App.ActiveDocument.Objects: - if (is_object_solid(el)): + if is_object_solid(el): match type.value: case EXPORT_TYPES.STL.value: - Mesh.export([el], path + '/' + FolderGenerator.SDF.value + - '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.stl') - meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \ - '/' + el.Label + '.stl' + Mesh.export( + [el], + path + + "/" + + FolderGenerator.SDF.value + + "/" + + FolderGenerator.MESHES.value + + "/" + + el.Label + + ".stl", + ) + meshes[el.Label] = ( + "/" + FolderGenerator.MESHES.value + "/" + el.Label + ".stl" + ) # case EXPORT_TYPES.DAO.value: # importDAE.export([el], path + '/' + FolderGenerator.SDF.value + # '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae') case EXPORT_TYPES.OBJ.value: import importOBJ - importOBJ.export([el], path + '/' + FolderGenerator.SDF.value + - '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.obj') - meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \ - '/' + el.Label + '.obj' - print(300) + + importOBJ.export( + [el], + path + + "/" + + FolderGenerator.SDF.value + + "/" + + FolderGenerator.MESHES.value + + "/" + + el.Label + + ".obj", + ) + meshes[el.Label] = ( + "/" + FolderGenerator.MESHES.value + "/" + el.Label + ".obj" + ) return meshes diff --git a/cad_stability_check/main.py b/cad_stability_check/main.py index d0f91a2..f5d0f1f 100644 --- a/cad_stability_check/main.py +++ b/cad_stability_check/main.py @@ -3,7 +3,6 @@ from typing import Any, TypeVar, Type, cast import FreeCAD as App import json import importOBJ -import FreeCAD as App import Draft import os import Part @@ -11,21 +10,28 @@ import numpy as np from typing import TypeAlias import FreeCADGui as Gui -ISequencesUnion: TypeAlias = dict[str:dict[str:list[str]]] +ISequencesUnion: TypeAlias = dict[str : dict[str : list[str]]] def importObjAtPath(path: str, inc: int): - importOBJ.insert(u"" + path, App.ActiveDocument.Label) + try: + importOBJ.insert("" + path, App.ActiveDocument.Label) - mesh = App.ActiveDocument.Objects[inc] - shape = Part.Shape() - shape.makeShapeFromMesh(mesh.Mesh.Topology, 0.05) - solid = Part.makeSolid(shape) - Part.show(solid) - App.ActiveDocument.Objects[inc + - 1].Label = App.ActiveDocument.Objects[inc].Name - App.ActiveDocument.removeObject(App.ActiveDocument.Objects[inc].Name) - return App.ActiveDocument.Objects[inc] + mesh = App.ActiveDocument.Objects[inc] + shape = Part.Shape() + shape.makeShapeFromMesh(mesh.Mesh.Topology, 0.05) + solid = Part.makeSolid(shape) + + Part.show(solid) + + App.ActiveDocument.Objects[inc + 1].Label = App.ActiveDocument.Objects[inc].Name + App.ActiveDocument.removeObject(App.ActiveDocument.Objects[inc].Name) + return App.ActiveDocument.Objects[inc] + except: + print("path") + print(path) + print("inc") + print(inc) T = TypeVar("T") @@ -52,15 +58,18 @@ def to_class(c: Type[T], x: Any) -> dict: def euler_to_quaternion(yaw, pitch, roll): - - qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - \ - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) - qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + \ - np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - \ - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) - qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + \ - np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos( + roll / 2 + ) * np.sin(pitch / 2) * np.sin(yaw / 2) + qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin( + roll / 2 + ) * np.cos(pitch / 2) * np.sin(yaw / 2) + qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin( + roll / 2 + ) * np.sin(pitch / 2) * np.cos(yaw / 2) + qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin( + roll / 2 + ) * np.sin(pitch / 2) * np.sin(yaw / 2) return [qx, qy, qz, qw] @@ -76,7 +85,7 @@ class Coords: self.z = z @staticmethod - def from_dict(obj: Any) -> 'Coords': + def from_dict(obj: Any) -> "Coords": assert isinstance(obj, dict) x = from_float(obj.get("x")) y = from_float(obj.get("y")) @@ -96,26 +105,25 @@ class Coords: class MotionResultModel: id: str - euler: Coords + quaternion: Coords position: Coords - def __init__(self, id: str, euler: Coords, position: Coords) -> None: + def __init__(self, id: str, quaternion: Coords, position: Coords) -> None: self.id = id - self.euler = euler + self.quaternion = quaternion self.position = position @staticmethod - def from_dict(obj: Any) -> 'MotionResultModel': - assert isinstance(obj, dict) + def from_dict(obj: Any) -> "MotionResultModel": id = from_str(obj.get("id")) - euler = Coords.from_dict(obj.get("euler")) + quaternion = Coords.from_dict(obj.get("quaternion")) position = Coords.from_dict(obj.get("position")) - return MotionResultModel(id, euler, position) + return MotionResultModel(id, quaternion, position) def to_dict(self) -> dict: result: dict = {} result["id"] = from_str(self.id) - result["euler"] = to_class(Coords, self.euler) + result["quaternion"] = to_class(Coords, self.quaternion) result["position"] = to_class(Coords, self.position) return result @@ -131,117 +139,144 @@ class SequencesEvaluation: pass def assemblyComputed(self): - debug = True + isOk = True for sequenceNumber, v in self.sequences.items(): for assemblyNumber, assemblySequenced in v.items(): - # print(assemblyNumber) - # print() - # if(assemblyNumber == 1 and sequenceNumber == 4 or assemblyNumber == 1 and sequenceNumber == 0): - if(assemblyNumber == 1 and sequenceNumber == 1 and debug): - debug = False - if(sequenceNumber == 0): - sequenceNumber+=1 - print(assemblySequenced) + if isOk: + # sequenceNumber += 1 + isOk = False self.comptedAssembly( - assemblySequenced, sequenceNumber, assemblyNumber) + assemblySequenced, sequenceNumber, assemblyNumber + ) pass - def comptedAssembly(self, assembly: list[str], sequenceNumber: int, assemblyNumber: int): + def comptedAssembly( + self, assembly: list[str], sequenceNumber: int, assemblyNumber: int + ): assemblyParts = [] for counter in range(len(assembly)): importObjAtPath( - self.assemblyDir + 'sdf/meshes/' + assembly[counter] + '.obj', - counter + self.assemblyDir + "generation/meshes/" + assembly[counter] + ".obj", + counter, + ) + assemblyParts.append( + {"part": App.ActiveDocument.Objects[counter], "name": assembly[counter]} ) - assemblyParts.append({ - "part": App.ActiveDocument.Objects[counter], - "name": assembly[counter] - }) - motionResult = json.loads((open(self.assemblyDir + 'stability' + '/' + str( - sequenceNumber + 1) + '/' + str(assemblyNumber) + '/' + 'motion_result.json')).read()) + motionResult = json.loads( + ( + open( + self.assemblyDir + + "stability" + + "/" + + str(sequenceNumber + 1) + + "/" + + str(assemblyNumber) + + "/" + + "motion_result.json" + ) + ).read() + ) - simulatorMotionResults: list['MotionResultModel'] = [] + simulatorMotionResults: list["MotionResultModel"] = [] for _k, v in motionResult.items(): + print(v) simulatorMotionResults.append(MotionResultModel.from_dict(v)) for el in simulatorMotionResults: for e in assemblyParts: # сопоставляем детали - if (el.id == e.get('name')): + if el.id == e.get("name"): # вычисляем центр детали для перемещения - center = e.get('part').Shape.CenterOfMass + center = e.get("part").Shape.CenterOfMass # получаем центр деталей из симуляции new_center = App.Vector(el.position.vector()) # вычисляем вектор смещения offset = new_center - center # перемещаем деталь на вектор смещения - e.get('part').Placement.Base += offset + e.get("part").Placement.Base += offset + + # # импортируем меш связанный с зоной обьекта + zonePart = importObjAtPath( + self.assemblyDir + + "stability/zones/meshes/zone_sub_assembly" + + str(assembly.__len__() - 1) + + ".obj", + len(App.ActiveDocument.Objects), + ) + # # получаем координаты зоны относительно детали за которой закреплена зона - # импортируем меш связанный с зоной обьекта - # zonePart = importObjAtPath(self.assemblyDir + "stability/zones/meshes/zone_sub_assembly" + str( - # assembly.__len__() - 1) + ".obj", len(App.ActiveDocument.Objects)) - # получаем координаты зоны относительно детали за которой закреплена зона - print(assemblyNumber) coords = json.loads( - (open(self.assemblyDir + "stability/zones/sub_assembly_coords_" + str(assemblyNumber - 1) + ".json")).read()) + ( + open( + self.assemblyDir + + "stability/zones/sub_assembly_coords_" + + str(assemblyNumber - 1) + + ".json" + ) + ).read() + ) assemblyCounter = len(assemblyParts) - detailWhichZoneBindings = assemblyParts[assemblyCounter - 2].get( - 'part') + detailWhichZoneBindings = assemblyParts[assemblyCounter - 2].get("part") - detailStabilityComputed = assemblyParts[assemblyCounter - 1].get( - 'part') - relativeCoordinates = coords.get('relativeCoordinates') + detailStabilityComputed = assemblyParts[assemblyCounter - 1].get("part") + relativeCoordinates = coords.get("relativeCoordinates") - relativeEuler = coords.get('relativeEuler') + relativeEuler = coords.get("relativeEuler") specificEuler = { - 'yaw': detailWhichZoneBindings.Placement.Rotation.toEuler()[0] - relativeEuler.get('yaw'), - 'pitch': detailWhichZoneBindings.Placement.Rotation.toEuler()[1] - relativeEuler.get('pitch'), - 'roll': detailWhichZoneBindings.Placement.Rotation.toEuler()[2] - relativeEuler.get('roll') + "yaw": detailWhichZoneBindings.Placement.Rotation.toEuler()[0] + - relativeEuler.get("yaw"), + "pitch": detailWhichZoneBindings.Placement.Rotation.toEuler()[1] + - relativeEuler.get("pitch"), + "roll": detailWhichZoneBindings.Placement.Rotation.toEuler()[2] + - relativeEuler.get("roll"), } - quaternion = euler_to_quaternion(specificEuler.get( - 'yaw'), specificEuler.get('pitch'), specificEuler.get('roll')) + quaternion = euler_to_quaternion( + specificEuler.get("yaw"), + specificEuler.get("pitch"), + specificEuler.get("roll"), + ) rotation = App.Rotation( - quaternion[0], quaternion[1], quaternion[2], quaternion[3]) + quaternion[0], quaternion[1], quaternion[2], quaternion[3] + ) detailStabilityComputed.Placement.Rotation = rotation centerVector = detailWhichZoneBindings.Shape.CenterOfMass - vector = App.Vector(relativeCoordinates.get( - 'x'), relativeCoordinates.get('y'), relativeCoordinates.get('z')) + vector = App.Vector( + relativeCoordinates.get("x"), + relativeCoordinates.get("y"), + relativeCoordinates.get("z"), + ) + print(vector) + # TODO + current_center = zonePart.Shape.CenterOfMass + move_vector = App.Vector(centerVector + vector) - current_center + zonePart.Placement.move(move_vector) - # current_center = zonePart.Shape.CenterOfMass - # move_vector = App.Vector(centerVector + vector) - current_center - # zonePart.Placement.move(move_vector) - - # computedStabilityResult = computedStability( - # zonePart, detailStabilityComputed) + computedStabilityResult = computedStability(zonePart, detailStabilityComputed) if sequenceNumber not in self.result.keys(): self.result[sequenceNumber] = [] - # self.result[sequenceNumber].append({ - # str(assemblyNumber): assembly, - # "result": computedStabilityResult - # }) - + self.result[sequenceNumber].append( + {str(assemblyNumber): assembly, "result": computedStabilityResult} + ) # for part in App.ActiveDocument.Objects: # App.ActiveDocument.removeObject(part.Name) def get_part_center(part): shape = None - if not hasattr(part, 'Shape'): + if not hasattr(part, "Shape"): shape = part.Mesh - if hasattr(part, 'Shape'): + if hasattr(part, "Shape"): shape = part.Shape center = shape.BoundBox.Center - return App.Vector(center[0], - center[1], - center[2]) + return App.Vector(center[0], center[1], center[2]) def move_second_part_to_match_center(first_part, second_part): @@ -255,8 +290,10 @@ def create(part): clone = Draft.make_clone([part], forcedraft=True) clone.Scale = App.Vector(1.30, 1.30, 1.30) - clone_corr = (App.Vector(0.4476673941774023, -2.109332894191716, -0.5918687740295264) - - clone.Placement.Base).scale(*App.Vector(-0.25, -0.25, -0.25)) + clone_corr = ( + App.Vector(0.4476673941774023, -2.109332894191716, -0.5918687740295264) + - clone.Placement.Base + ).scale(*App.Vector(-0.25, -0.25, -0.25)) clone.Placement.move(clone_corr) App.ActiveDocument.recompute() @@ -264,77 +301,94 @@ def create(part): def getFullPathObj(assemblyFolder: str, name: str): - return assemblyFolder + 'sdf/meshes/' + name + '.obj' + return assemblyFolder + "sdf/meshes/" + name + ".obj" def computedStability(refElement, childElement): rootElement = childElement.Shape.BoundBox # Создание обьекта на котором делается операция пересечения App.activeDocument().addObject("Part::MultiCommon", "Common") - App.activeDocument().Common.Shapes = [refElement, childElement, ] - App.ActiveDocument.getObject('Common').ViewObject.ShapeColor = getattr(App.ActiveDocument.getObject( - refElement.Name).getLinkedObject(True).ViewObject, 'ShapeColor', App.ActiveDocument.getObject('Common').ViewObject.ShapeColor) - App.ActiveDocument.getObject('Common').ViewObject.DisplayMode = getattr(App.ActiveDocument.getObject( - childElement.Name).getLinkedObject(True).ViewObject, 'DisplayMode', App.ActiveDocument.getObject('Common').ViewObject.DisplayMode) + App.activeDocument().Common.Shapes = [ + refElement, + childElement, + ] + App.ActiveDocument.getObject("Common").ViewObject.ShapeColor = getattr( + App.ActiveDocument.getObject(refElement.Name).getLinkedObject(True).ViewObject, + "ShapeColor", + App.ActiveDocument.getObject("Common").ViewObject.ShapeColor, + ) + App.ActiveDocument.getObject("Common").ViewObject.DisplayMode = getattr( + App.ActiveDocument.getObject(childElement.Name) + .getLinkedObject(True) + .ViewObject, + "DisplayMode", + App.ActiveDocument.getObject("Common").ViewObject.DisplayMode, + ) App.ActiveDocument.recompute() - obj = App.ActiveDocument.getObjectsByLabel('Common')[0] + obj = App.ActiveDocument.getObjectsByLabel("Common")[0] shp = obj.Shape bbox = shp.BoundBox # Если после операции пересечения зона обьекта совпадает с зоной тестируемого обьекта то тест прошел успешно - if bbox.XLength == rootElement.XLength and bbox.YLength == rootElement.YLength and rootElement.ZLength == bbox.ZLength: + if ( + bbox.XLength == rootElement.XLength + and bbox.YLength == rootElement.YLength + and rootElement.ZLength == bbox.ZLength + ): return True return False def autoStabilityZoneComputed(stepFilesPaths: list[str], directoryStableZonesPath: str): - cadObjects = [] - for count in range(len(stepFilesPaths)): - importObjAtPath(stepFilesPaths[count], count) cadObjects.append(App.ActiveDocument.Objects[count]) assemblesBindings = [] for increment in range(len(cadObjects)): - if (increment != 0): + if increment != 0: detailForEvaluationZ = cadObjects[increment] - zoneBindingDetailZ = cadObjects[increment-1] + zoneBindingDetailZ = cadObjects[increment - 1] assemblesBindings.append( - {'zoneBindingDetail': detailForEvaluationZ, 'detailForEvaluation': zoneBindingDetailZ, 'relativeCoordinates': None, 'zonePart': None}) + { + "zoneBindingDetail": detailForEvaluationZ, + "detailForEvaluation": zoneBindingDetailZ, + "relativeCoordinates": None, + "zonePart": None, + } + ) for increment in range(len(assemblesBindings)): - el = assemblesBindings[increment] - zoneBindingDetail = el.get('zoneBindingDetail') + zoneBindingDetail = el.get("zoneBindingDetail") zoneBindingDetailCenterVector = zoneBindingDetail.Shape.CenterOfMass - zoneDetail = create(el.get('detailForEvaluation')) - - move_second_part_to_match_center( - el.get('zoneBindingDetail'), zoneDetail) - zoneDetail.Label = 'zone_sub_assembly' + str(increment + 1) + zoneDetail = create(el.get("detailForEvaluation")) + move_second_part_to_match_center(el.get("zoneBindingDetail"), zoneDetail) + zoneDetail.Label = "zone_sub_assembly" + str(increment + 1) zoneDetail.ViewObject.ShapeColor = (0.40, 0.74, 0.71) zoneDetail.ViewObject.Transparency = 50 - zoneDetailCenterVector = el.get( - 'detailForEvaluation').Shape.CenterOfMass + zoneDetailCenterVector = el.get("detailForEvaluation").Shape.CenterOfMass - el['relativeCoordinates'] = { - 'x': zoneBindingDetailCenterVector.x - zoneDetailCenterVector.x, - 'y': zoneBindingDetailCenterVector.y - zoneDetailCenterVector.y, - 'z': zoneBindingDetailCenterVector.z - zoneDetailCenterVector.z + el["relativeCoordinates"] = { + "x": zoneBindingDetailCenterVector.x - zoneDetailCenterVector.x, + "y": zoneBindingDetailCenterVector.y - zoneDetailCenterVector.y, + "z": zoneBindingDetailCenterVector.z - zoneDetailCenterVector.z, } - el['relativeEuler'] = { - 'yaw': zoneBindingDetail.Placement.Rotation.toEuler()[0] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[0], - 'pitch': zoneBindingDetail.Placement.Rotation.toEuler()[1] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[1], - 'roll': zoneBindingDetail.Placement.Rotation.toEuler()[2] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[2] + el["relativeEuler"] = { + "yaw": zoneBindingDetail.Placement.Rotation.toEuler()[0] + - el.get("detailForEvaluation").Placement.Rotation.toEuler()[0], + "pitch": zoneBindingDetail.Placement.Rotation.toEuler()[1] + - el.get("detailForEvaluation").Placement.Rotation.toEuler()[1], + "roll": zoneBindingDetail.Placement.Rotation.toEuler()[2] + - el.get("detailForEvaluation").Placement.Rotation.toEuler()[2], } - el['zonePart'] = zoneDetail + el["zonePart"] = zoneDetail - meshesPath = directoryStableZonesPath + 'meshes/' + meshesPath = directoryStableZonesPath + "meshes/" if not os.path.exists(directoryStableZonesPath): os.makedirs(directoryStableZonesPath) if not os.path.exists(meshesPath): @@ -342,23 +396,27 @@ def autoStabilityZoneComputed(stepFilesPaths: list[str], directoryStableZonesPat zonesSaved = {} for counter in range(len(assemblesBindings)): zoneComputed = assemblesBindings[counter] - mesh = zoneComputed.get('zonePart') - zonesSavePath = meshesPath + mesh.Label + '.obj' + mesh = zoneComputed.get("zonePart") + zonesSavePath = meshesPath + mesh.Label + ".obj" importOBJ.export([mesh], zonesSavePath) - zonesSaved[mesh.Label] = 'meshes/' + mesh.Label + '.obj' + zonesSaved[mesh.Label] = "meshes/" + mesh.Label + ".obj" for counter in range(len(assemblesBindings)): el = assemblesBindings[counter] - savePath = zonesSaved[el.get('zonePart').Label] - el['zonePart'] = savePath - el['detailForEvaluation'] = el['detailForEvaluation'].Label - el['zoneBindingDetail'] = el['zoneBindingDetail'].Label + savePath = zonesSaved[el.get("zonePart").Label] + el["zonePart"] = savePath + el["detailForEvaluation"] = el["detailForEvaluation"].Label + el["zoneBindingDetail"] = el["zoneBindingDetail"].Label json_result = json.dumps(el) - file_to_open = directoryStableZonesPath + \ - 'sub_assembly_coords_' + str(counter) + '.json' + file_to_open = ( + directoryStableZonesPath + "sub_assembly_coords_" + str(counter) + ".json" + ) - f = open(file_to_open, 'w', ) + f = open( + file_to_open, + "w", + ) f.write(json_result) f.close() @@ -366,40 +424,38 @@ def autoStabilityZoneComputed(stepFilesPaths: list[str], directoryStableZonesPat def main(): App.newDocument() - env = json.loads((open('./env.json')).read()) + env = json.loads((open("./env.json")).read()) - assemblyDir = env.get('aspPath') - sequencesJSON = json.loads((open(assemblyDir + 'sequences.json')).read()) - directoryStableZones = assemblyDir + 'stability/zones/' + assemblyDir = env.get("aspPath") + sequencesJSON = json.loads((open(assemblyDir + "sequences.json")).read()) + directoryStableZones = assemblyDir + "stability/zones/" - sequences = sequencesJSON.get('sequences') - stepStructure = json.loads( - (open(assemblyDir + 'step-structure.json')).read()) + sequences = sequencesJSON.get("sequences") + stepStructure = json.loads((open(assemblyDir + "step-structure.json")).read()) stepFilesPaths = [] for step in stepStructure: - stepFilesPaths.append(assemblyDir+'sdf/meshes/' + step + '.obj') + stepFilesPaths.append(assemblyDir + "generation/meshes/" + step + ".obj") if not os.path.exists(directoryStableZones): - print('Zones not found automatic calculation started') + print("Zones not found automatic calculation started") autoStabilityZoneComputed(stepFilesPaths, directoryStableZones) sequencesJoin = {} for arrayCounter in range(len(sequences)): for indexCounter in range(len(sequences[arrayCounter])): - if (indexCounter != 0): - if (sequencesJoin.get(arrayCounter) == None): + if indexCounter != 0: + if sequencesJoin.get(arrayCounter) == None: sequencesJoin[arrayCounter] = { - indexCounter: sequences[arrayCounter][0:indexCounter+1] + indexCounter: sequences[arrayCounter][0 : indexCounter + 1] } else: - sequencesJoin[arrayCounter][indexCounter] = sequences[arrayCounter][0:indexCounter+1] - - - + sequencesJoin[arrayCounter][indexCounter] = sequences[arrayCounter][ + 0 : indexCounter + 1 + ] + seqEvaluation = SequencesEvaluation(sequencesJoin, assemblyDir) seqEvaluation.assemblyComputed() - print(seqEvaluation.result) main() diff --git a/cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py b/cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py index eb40dfe..13349e7 100644 --- a/cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py +++ b/cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py @@ -18,7 +18,7 @@ import shutil class RobossemblerFreeCadExportScenario: def call(self): - + print(2012) path = self.qtGuiFeature() if path == None: return diff --git a/geometric_feasibility_predicate/README.MD b/geometric_feasibility_predicate/README.MD index 7185164..60daaf6 100644 --- a/geometric_feasibility_predicate/README.MD +++ b/geometric_feasibility_predicate/README.MD @@ -1 +1,2 @@ +/Users/idontsudo/Desktop/FreeCAD.app/Contents/MacOS/FreeCAD freecadcmd main.py \ No newline at end of file diff --git a/geometric_feasibility_predicate/env.json b/geometric_feasibility_predicate/env.json index 9c62939..2203e0c 100644 --- a/geometric_feasibility_predicate/env.json +++ b/geometric_feasibility_predicate/env.json @@ -1,4 +1,5 @@ { - "cadFilePath":"/home/idontsudo/framework/asp/out/disk_and_axis_n (2).FCStd", - "outPath":"/home/idontsudo/framework/asp/out/" -} \ No newline at end of file + "cadFilePath": "/Users/idontsudo/Desktop/asp-example/disk_and_axis_n.FCStd", + "outPath": "/Users/idontsudo/Desktop/asp-example/", + "objectIndentation": 0 +} diff --git a/geometric_feasibility_predicate/main.py b/geometric_feasibility_predicate/main.py index d8840ab..690bc2a 100644 --- a/geometric_feasibility_predicate/main.py +++ b/geometric_feasibility_predicate/main.py @@ -6,6 +6,16 @@ from typing import List, Dict, Any, TypeVar, Callable, Type, cast from itertools import repeat +class CoreList(List): + # the list contains only True + def onlyTrue(self) -> bool: + print(self) + for el in self: + if el is not True: + return False + return True + + def isInListRange(listIn, index): try: listIn[index] @@ -31,8 +41,12 @@ class AllSequences: inc = 0 for matrix in self.all_sequences: for index in range(len(matrix)): - result[inc][index] = list(filter(lambda el: el.get( - 'number') == matrix[index]+1, self.topologyIds))[0].get('name') + result[inc][index] = list( + filter( + lambda el: el.get("number") == matrix[index] + 1, + self.topologyIds, + ) + )[0].get("name") inc += 1 self.adj_matrix_names = result pass @@ -59,11 +73,12 @@ class AllSequences: def findId(self, listMatrix, id): def filter_odd_num(in_num): - if in_num['name'] == id: + if in_num["name"] == id: return True else: return False - return list(filter(filter_odd_num, listMatrix))[0]['number'] + + return list(filter(filter_odd_num, listMatrix))[0]["number"] def iter_paths(self, adj, min_length=6, path=None): if not path: @@ -94,7 +109,8 @@ class AllSequences: for k, v in matrix.items(): inc += 1 topologyMatrixNumber[inc] = list( - map(lambda el: self.findId(topologyIds, el), v)) + map(lambda el: self.findId(topologyIds, el), v) + ) self.topologyIds = topologyIds adj = [] matrixSize = matrix.keys().__len__() @@ -102,18 +118,121 @@ class AllSequences: for k, v in topologyMatrixNumber.items(): adj.append(list(repeat(0, matrixSize))) for el in v: - adj[inc][el-1] = 1 + adj[inc][el - 1] = 1 inc += 1 return self.find_all_sequences(adj) +class VectorModel: + x: float + y: float + z: float + + def __init__(self, cadVector) -> None: + self.x = cadVector[0] + self.y = cadVector[1] + self.z = cadVector[2] + pass + + def toFreeCadVector(self): + return App.Vector(self.x, self.y, self.z) + + class FreeCadRepository: _solids = [] + def isAllObjectsSolids(self) -> List[str]: + result = [] + for part in App.ActiveDocument.Objects: + if self.is_object_solid(part) is False: + result.append(part.Label) + return result + + def objectSetPosition(self, solid, cadVector): + solid.Placement.Base = cadVector + pass + + def objectGetPosition(self, solid) -> VectorModel: + return VectorModel(cadVector=solid.Placement.Base) + + def isObjectIntersections(self, part) -> bool: + for solid in self.getAllSolids(): + if solid.Label != part.Label: + collisionResult: int = int(part.Shape.distToShape(solid.Shape)[0]) + if collisionResult == 0: + return True + return False + + def objectHasTouches(self, part, objectIndentation: float) -> List[str]: + positionVector = self.objectGetPosition(part) + result = CoreList() + result.append(self.isObjectIntersections(part=part)) + + if objectIndentation != 0 and objectIndentation != None: + result.append( + self.axis_movement_and_intersections_observer( + positionVector=positionVector, + alongAxis="x", + objectIndentation=objectIndentation, + part=part, + ) + ) + result.append( + self.axis_movement_and_intersections_observer( + positionVector=positionVector, + alongAxis="y", + objectIndentation=objectIndentation, + part=part, + ) + ) + result.append( + self.axis_movement_and_intersections_observer( + positionVector=positionVector, + alongAxis="z", + objectIndentation=objectIndentation, + part=part, + ) + ) + + return result.onlyTrue() + + def axis_movement_and_intersections_observer( + self, + positionVector: VectorModel, + alongAxis: str, + objectIndentation: float, + part, + ) -> bool: + # UP + positionVector.__setattr__( + alongAxis, positionVector.__getattribute__(alongAxis) + objectIndentation + ) + self.objectSetPosition(part, positionVector.toFreeCadVector()) + result = self.isObjectIntersections(part=part) + if result: + return True + # DOWN + positionVector.__setattr__( + alongAxis, positionVector.__getattribute__(alongAxis) - objectIndentation + ) + positionVector.__setattr__( + alongAxis, positionVector.__getattribute__(alongAxis) - objectIndentation + ) + result = self.isObjectIntersections(part=part) + if result: + return True + self.isObjectIntersections(part=part) + # ROLLBACK + positionVector.__setattr__( + alongAxis, positionVector.__getattribute__(alongAxis) + objectIndentation + ) + self.objectSetPosition(part, positionVector.toFreeCadVector()) + return False + def getAllSolids(self): - if (self._solids.__len__() == 0): + if self._solids.__len__() == 0: for part in App.ActiveDocument.Objects: - if (self.is_object_solid(part)): + if self.is_object_solid(part): self._solids.append(part) return self._solids @@ -121,14 +240,12 @@ class FreeCadRepository: def is_object_solid(self, obj): if not isinstance(obj, App.DocumentObject): return False - if hasattr(obj, 'Group'): + if hasattr(obj, "Group"): return False - if not hasattr(obj, 'Shape'): + if not hasattr(obj, "Shape"): return False - if not hasattr(obj.Shape, 'Mass'): - return False - if not hasattr(obj.Shape, 'Solids'): + if not hasattr(obj.Shape, "Solids"): return False if len(obj.Shape.Solids) == 0: @@ -159,6 +276,7 @@ def to_class(c: Type[T], x: Any) -> dict: assert isinstance(x, c) return cast(Any, x).to_dict() + # Вспомогательный класс который делает генрацию JSON на основе пайтон обьектов @@ -168,7 +286,9 @@ class AdjacencyMatrix: first_detail: str matrix: Dict[str, List[str]] - def __init__(self, all_parts: List[str], first_detail: str, matrix: Dict[str, List[str]]) -> None: + def __init__( + self, all_parts: List[str], first_detail: str, matrix: Dict[str, List[str]] + ) -> None: self.all_parts = all_parts self.first_detail = first_detail self.matrix = matrix @@ -183,11 +303,11 @@ class AdjacencyMatrix: def validateMatrix(self): for el in self.all_parts: - if (self.matrix.get(el) == None): - self.matrixError[el] = 'Not found adjacency ' + el + if self.matrix.get(el) == None: + self.matrixError[el] = "Not found adjacency " + el @staticmethod - def from_dict(obj: Any) -> 'AdjacencyMatrix': + def from_dict(obj: Any) -> "AdjacencyMatrix": assert isinstance(obj, dict) all_pars = from_list(from_str, obj.get("allParts")) first_detail = from_str(obj.get("firstDetail")) @@ -199,12 +319,11 @@ class AdjacencyMatrix: result: dict = {} result["allParts"] = from_list(from_str, self.all_parts) result["firstDetail"] = from_str(self.first_detail) - result["matrix"] = from_dict( - lambda x: from_list(from_str, x), self.matrix) - if (self.matrixError.values().__len__() == 0): - result['matrixError'] = None + result["matrix"] = from_dict(lambda x: from_list(from_str, x), self.matrix) + if self.matrixError.values().__len__() == 0: + result["matrixError"] = None else: - result['matrixError'] = self.matrixError + result["matrixError"] = self.matrixError return result def getDictMatrix(self) -> dict: @@ -225,11 +344,11 @@ def adjacency_matrix_from_dict(s: Any) -> AdjacencyMatrix: def adjacency_matrix_to_dict(x: AdjacencyMatrix) -> Any: return to_class(AdjacencyMatrix, x) + # Вспомогательный класс для работы с Freecad class FreeCadMetaModel(object): - def __init__(self, label, vertex) -> None: self.label = label self.vertex = vertex @@ -240,12 +359,18 @@ collision_squares_labels = [] class MeshGeometryCoordinateModel(object): # Получение геометрии мешей - def __init__(self, x, y, z, label,): + def __init__( + self, + x, + y, + z, + label, + ): self.x = x self.y = y self.z = z self.label = label - self.cadLabel = '' + self.cadLabel = "" def initializePrimitivesByCoordinate(self, detailSquares): uuidDoc = str(uuid.uuid1()) @@ -259,8 +384,9 @@ class MeshGeometryCoordinateModel(object): part.Length = 2 part.Placement = App.Placement( App.Vector(self.x - 1, self.y - 1, self.z - 1), - App.Rotation(App.Vector(0.00, 0.00, 1.00), 0.00)) - if (detailSquares.get(self.label) is None): + App.Rotation(App.Vector(0.00, 0.00, 1.00), 0.00), + ) + if detailSquares.get(self.label) is None: detailSquares[self.label] = [] detailSquares[self.label].append(self) self.cadLabel = uuidDoc @@ -272,19 +398,21 @@ class FS: return json.loads((open(path)).read()) def writeFile(data, filePath, fileName): - file_to_open = filePath + fileName - f = open(file_to_open, 'w', encoding='utf8') + f = open(file_to_open, "w", encoding="utf8") f.write(data) def readFile(path: str): return open(path).read() - def readFilesTypeFolder(pathFolder: str, fileType='.json'): + def readFilesTypeFolder(pathFolder: str, fileType=".json"): filesJson = list( - filter(lambda x: x[-fileType.__len__():] == fileType, os.listdir(pathFolder))) + filter( + lambda x: x[-fileType.__len__() :] == fileType, os.listdir(pathFolder) + ) + ) return filesJson @@ -312,20 +440,20 @@ class GetCollisionAtPrimitiveUseCase(object): for model in freeCadMetaModels: activePart = App.ActiveDocument.getObjectsByLabel(model.label)[0] for key in detailSquares: - if (model.label != key): + if model.label != key: for renderPrimitive in detailSquares[key]: primitivePart = App.ActiveDocument.getObjectsByLabel( - renderPrimitive.cadLabel)[0] + renderPrimitive.cadLabel + )[0] collisionResult: int = int( - activePart.Shape.distToShape(primitivePart.Shape)[0]) - if (collisionResult == 0): + activePart.Shape.distToShape(primitivePart.Shape)[0] + ) + if collisionResult == 0: if matrix.get(model.label) == None: matrix[model.label] = [renderPrimitive.label] else: if isUnique(matrix[model.label], renderPrimitive.label): - matrix[model.label].append( - renderPrimitive.label - ) + matrix[model.label].append(renderPrimitive.label) return matrix @@ -354,21 +482,23 @@ class GetPartPrimitiveCoordinatesUseCase(object): return meshCoordinates -class InitPartsParseUseCase(): +class InitPartsParseUseCase: # Инициализация парсинга def call(self): product_details = [] for part in FreeCadRepository().getAllSolids(): if part is not None: model = FreeCadMetaModel(part.Label, part.Shape.Vertexes) - if (model is not None): + if model is not None: product_details.append(model) return product_details class RenderPrimitiveUseCase(object): # Рендеринг премитивов - def call(self, meshModels: list[MeshGeometryCoordinateModel], detailSquares) -> None: + def call( + self, meshModels: list[MeshGeometryCoordinateModel], detailSquares + ) -> None: for mesh in meshModels: mesh.initializePrimitivesByCoordinate(detailSquares) @@ -379,12 +509,12 @@ class ClearWorkSpaceDocumentUseCase(object): for key in detailSquares: for renderPrimitive in detailSquares[key]: primitivePart = App.ActiveDocument.getObjectsByLabel( - renderPrimitive.cadLabel)[0] + renderPrimitive.cadLabel + )[0] App.ActiveDocument.removeObject(primitivePart.Name) class RenderPrimitivesScenario(object): - def __init__( self, initPartsParseUseCase: InitPartsParseUseCase, @@ -416,7 +546,8 @@ class ClearWorkSpaceDocumentUseCase(object): for key in detailSquares: for renderPrimitive in detailSquares[key]: primitivePart = App.ActiveDocument.getObjectsByLabel( - renderPrimitive.cadLabel)[0] + renderPrimitive.cadLabel + )[0] App.ActiveDocument.removeObject(primitivePart.Name) @@ -436,6 +567,7 @@ class CadAdjacencyMatrix: first_detail=GetFirstDetailUseCase().call(), matrix=matrix, ) + # Матрица основанная на соприкосновении обьектов def matrixBySurfaces(self): @@ -446,12 +578,17 @@ class CadAdjacencyMatrix: if part.Label != nextPart.Label: # Вычисление соприконсоновения площади деталей collisionResult: int = int( - part.Shape.distToShape(nextPart.Shape)[0]) - if (collisionResult == 0): + part.Shape.distToShape(nextPart.Shape)[0] + ) + print(collisionResult) + print("collisionResult") + if collisionResult == 0: matrix[part.Label].append(nextPart.Label) - return AdjacencyMatrix(all_parts=GetAllPartsLabelsUseCase( - ).call(), first_detail=GetFirstDetailUseCase().call(), - matrix=matrix + + return AdjacencyMatrix( + all_parts=GetAllPartsLabelsUseCase().call(), + first_detail=GetFirstDetailUseCase().call(), + matrix=matrix, ) @@ -472,67 +609,166 @@ def to_ascii_hash(text): def matrixGetUniqueContact(matrix): - detailsToCheck = [] detailsHashCheck = {} for k, v in matrix.items(): for el in v: - if (el != k): + if el != k: hash = to_ascii_hash(k + el) - if (detailsHashCheck.get(hash) == None): + if detailsHashCheck.get(hash) == None: detailsHashCheck[hash] = hash - detailsToCheck.append({ - 'child': el, - 'parent': k - }) + detailsToCheck.append({"child": el, "parent": k}) return detailsToCheck -def intersectionComputed(parts): - App.activeDocument().addObject("Part::MultiCommon", "Common") - App.activeDocument().Common.Shapes = [parts[0], parts[1]] - App.activeDocument().getObject('Common').ViewObject.ShapeColor = getattr(parts[0].getLinkedObject(True).ViewObject, 'ShapeColor', App.activeDocument().getObject('Common').ViewObject.ShapeColor) - App.activeDocument().getObject('Common').ViewObject.DisplayMode = getattr(parts[0].getLinkedObject(True).ViewObject, 'DisplayMode', App.activeDocument().getObject('Common').ViewObject.DisplayMode) - App.ActiveDocument.recompute() - area = App.activeDocument().getObject('Common').Shape.Area - App.ActiveDocument.removeObject('Common') - return area - + +class IntersectionComputedUseCase: + def call(parts): + App.activeDocument().addObject("Part::MultiCommon", "Common") + App.activeDocument().Common.Shapes = [parts[0], parts[1]] + App.activeDocument().getObject("Common").ViewObject.ShapeColor = getattr( + parts[0].getLinkedObject(True).ViewObject, + "ShapeColor", + App.activeDocument().getObject("Common").ViewObject.ShapeColor, + ) + App.activeDocument().getObject("Common").ViewObject.DisplayMode = getattr( + parts[0].getLinkedObject(True).ViewObject, + "DisplayMode", + App.activeDocument().getObject("Common").ViewObject.DisplayMode, + ) + App.ActiveDocument.recompute() + area = App.activeDocument().getObject("Common").Shape.Area + App.ActiveDocument.removeObject("Common") + return area + + +class ErrorStringModel: + def __init__(self, error: str) -> None: + self.error = error + pass + + error: str + + def toString(self) -> str: + return json.dumps( + { + "error": self.error, + }, + ensure_ascii=False, + indent=4, + ) + + +class IsAllObjectSolidsCheckUseCase: + def call() -> ErrorStringModel: + result = FreeCadRepository().isAllObjectsSolids() + if result.__len__() == 0: + return None + + return ErrorStringModel(error="Is not solid objects: " + ",".join(result)) + + +class CheckObjectHasTouchesUseCase: + def call(objectIndentation: float) -> ErrorStringModel: + result = [] + for part in FreeCadRepository().getAllSolids(): + if ( + FreeCadRepository().objectHasTouches( + part=part, objectIndentation=objectIndentation + ) + is False + ): + result.append(part.Label) + if result.__len__() == 0: + return None + return ErrorStringModel( + error="Solids bodies have no recounts: " + ",".join(result) + ) + + +class CheckCadIntersectionObjects: + report = [] + + def call() -> bool: + FreeCadRepository().getAllSolids() + return False + + +class ExitFreeCadUseCase: + def call(): + import FreeCADGui as Gui + + freecadQTWindow = Gui.getMainWindow() + freecadQTWindow.close() + + +# class CheckValidIntersectionUseCase: +# def call() -> ErrorStringModel: +# for part in FreeCadRepository().getAllSolids(): +# print(part) +# FreeCadRepository().obj +# pass + + def main(): - env = FS.readJSON('env.json') - cadFile = env['cadFilePath'] - outPath = env['outPath'] - if (cadFile == None): - return TypeError('CadFile not found env.json') - App.open(u'' + cadFile) + env = FS.readJSON("env.json") + cadFilePath = str(env["cadFilePath"]) + outPath = str(env["outPath"]) + objectIndentation = float(env["objectIndentation"]) + + if cadFilePath == None: + return TypeError("CadFile not found env.json") + App.open("" + cadFilePath) + + # isAllObjectSolidsCheckUseCase = IsAllObjectSolidsCheckUseCase.call() + + # if isAllObjectSolidsCheckUseCase != None: + # FS.writeFile(isAllObjectSolidsCheckUseCase.toString(), outPath, 'error.json') + # ExitFreeCadUseCase.call() + # return + + # checkObjectHasTouchesUseCase = CheckObjectHasTouchesUseCase.call(objectIndentation) + + # if checkObjectHasTouchesUseCase != None: + # FS.writeFile(checkObjectHasTouchesUseCase.toString(), outPath, 'error.json') + # ExitFreeCadUseCase.call() + # return topologyMatrix = CadAdjacencyMatrix().matrixBySurfaces() import json - sequences = json.dumps({"sequences": AllSequences( - topologyMatrix.matrix).adj_matrix_names}, ensure_ascii=False, indent=4) + + sequences = json.dumps( + {"sequences": AllSequences(topologyMatrix.matrix).adj_matrix_names}, + ensure_ascii=False, + indent=4, + ) matrix = topologyMatrix.matrix contacts = matrixGetUniqueContact(matrix) - intersection_geometry = { - 'status':True, - 'recalculations':None - } + intersection_geometry = {"status": True, "recalculations": None} for el in contacts: - child = App.ActiveDocument.getObjectsByLabel(el.get('child'))[0] - parent = App.ActiveDocument.getObjectsByLabel(el.get('parent'))[0] - area = intersectionComputed([child,parent]) - if(area != 0.0): - if(intersection_geometry.get('recalculations') == None): - intersection_geometry['status'] = False - intersection_geometry['recalculations'] = [] - intersection_geometry['recalculations'].append({ - 'area':area, - 'connect': el.get('child') + ' ' + el.get('parent') - }) + child = App.ActiveDocument.getObjectsByLabel(el.get("child"))[0] + parent = App.ActiveDocument.getObjectsByLabel(el.get("parent"))[0] + area = IntersectionComputedUseCase.call([child, parent]) + if area != 0.0: + if intersection_geometry.get("recalculations") == None: + intersection_geometry["status"] = False + intersection_geometry["recalculations"] = [] + intersection_geometry["recalculations"].append( + {"area": area, "connect": el.get("child") + " " + el.get("parent")} + ) + + FS.writeFile( + json.dumps(intersection_geometry, ensure_ascii=False, indent=4), + outPath, + "intersection_geometry.json", + ) + FS.writeFile(sequences, outPath, "sequences.json") + + FS.writeFile( + json.dumps(topologyMatrix.to_dict(), ensure_ascii=False, indent=4), + outPath, + "adjacency_matrix.json", + ) + ExitFreeCadUseCase.call() + - - FS.writeFile(json.dumps(intersection_geometry, ensure_ascii=False, indent=4), outPath, 'intersection_geometry.json') - FS.writeFile(sequences, outPath, 'sequences.json') - FS.writeFile(json.dumps(topologyMatrix.to_dict(), - ensure_ascii=False, indent=4), outPath, 'adjacency_matrix.json') - main() - \ No newline at end of file diff --git a/insertion_vector_predicate/.gitignore b/insertion_vector_predicate/.gitignore new file mode 100644 index 0000000..4a47d9e --- /dev/null +++ b/insertion_vector_predicate/.gitignore @@ -0,0 +1,116 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +env/ +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +.hypothesis/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# pyenv +.python-version + +# celery beat schedule file +celerybeat-schedule + +# SageMath parsed files +*.sage.py + +# dotenv +.env + +# virtualenv +.venv +venv/ +ENV/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ + +# blender backup files +*.blend1 +install_plugin_cad.sh +.vscode +.DS_Store + +# emacs backup files +~* +*~ +*# +.#* +\#*\# +out/ +/env.json \ No newline at end of file diff --git a/insertion_vector_predicate/README.md b/insertion_vector_predicate/README.md new file mode 100644 index 0000000..570abdd --- /dev/null +++ b/insertion_vector_predicate/README.md @@ -0,0 +1,19 @@ +# Start dev +create env.json + +```json +{ + "cadDoc":"CAD_DOC_PATH_REPLACE", + "sequences":"SEQUENCES_PATH_REPLACE", + "aspDir":"ASP_DIR_REPLACE" +} +``` +# Command generation assets +freecad generate.py + +# Command generation insertion vectors + +git submodule update --init +conda env create -f assembly/environment.yml +conda activate assembly +python3 main.py \ No newline at end of file diff --git a/insertion_vector_predicate/generate.py b/insertion_vector_predicate/generate.py new file mode 100644 index 0000000..c08adb3 --- /dev/null +++ b/insertion_vector_predicate/generate.py @@ -0,0 +1,205 @@ + + +from typing import List +import FreeCAD as App +import Part +import Mesh +import Part +import MeshPart +import os +import json +import FreeCADGui as Gui + +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', encoding='utf-8', + errors='ignore') + f.write(data) + f.close() + def createFolder(path: str): + if (not os.path.exists(path)): + return os.mkdir(path) + + + +class SimpleCopyPartModel: + id = None + copyLink = None + label = None + part = None + + def getPart(self): + return self.part + + def __init__(self, part) -> None: + try: + from random import randrange + self.id = str(randrange(1000000)) + childObj = part + print(part) + __shape = Part.getShape( + childObj, '', needSubElement=False, refine=False) + obj = App.ActiveDocument.addObject('Part::Feature', self.id) + obj.Shape = __shape + self.part = obj + self.label = obj.Label + App.ActiveDocument.recompute() + except Exception as e: + print(e) + + def remove(self): + App.ActiveDocument.removeObject(self.label) + +class MeshPartModel: + id = None + mesh = None + + def __init__(self, part) -> None: + try: + from random import randrange + self.id = 'mesh' + str(randrange(1000000)) + document = App.ActiveDocument + mesh = document.addObject("Mesh::Feature", self.id) + shape = Part.getShape(part, "") + mesh.Mesh = MeshPart.meshFromShape( + Shape=shape, LinearDeflection=20, AngularDeflection=0.1, Relative=False) + mesh.Label = self.id + self.mesh = mesh + except Exception as e: + print(e) + pass + + def remove(self): + try: + App.ActiveDocument.removeObject(self.mesh.Label) + except Exception as e: + print(e) + + + +class JoinMeshModel: + id = None + mesh = None + + def __init__(self, meshesPartModels: list['MeshPartModel']) -> None: + meshes = [] + from random import randrange + for el in meshesPartModels: + meshes.append(el.mesh.Mesh) + + self.id = 'MergedMesh' + str(randrange(1000000)) + doc = App.ActiveDocument + merged_mesh = Mesh.Mesh() + for el in meshes: + merged_mesh.addMesh(el) + + new_obj = doc.addObject("Mesh::Feature", self.id) + new_obj.Mesh = merged_mesh + new_obj.ViewObject.DisplayMode = "Flat Lines" + self.mesh = new_obj + + def remove(self): + try: + App.ActiveDocument.removeObject(self.id) + except Exception as e: + print(e) + + +class ExportAssemblyThemAllUseCase: + + def call(self, path:str, assemblys:list[str]): + assembly = assemblys + asmStructure = {} + inc = 0 + for el in assembly: + if (inc != 0): + asmStructure[inc] = { + "child": el, + "parents": assembly[0:inc] + } + inc += 1 + objectsFreeCad = App.ActiveDocument.Objects + asmSolids = {} + for k, v in asmStructure.items(): + assemblyParentList = v['parents'] + assemblyChild = v['child'] + for el in assemblyParentList: + for solid in objectsFreeCad: + if (el == solid.Label): + if (asmSolids.get(k) is None): + + asmSolids[k] = {'parents': [], 'child': list( + filter(lambda x: x.Label == assemblyChild, objectsFreeCad))[0]} + + asmSolids[k]['parents'].append(solid) + + inc = 0 + for k, v in asmSolids.items(): + geometry = {"0": [], "1": []} + if (k != 0): + App.activeDocument().addObject("Part::Compound", "Compound") + + copyLinks = list( + map(lambda el: SimpleCopyPartModel(el), v['parents'])) + + if copyLinks != None: + App.activeDocument().Compound.Links = list( + map(lambda el: el.getPart(), copyLinks)) + + object = App.activeDocument().getObject('Compound') + boundBox = object.Shape.BoundBox + geometry['0'].append(boundBox.XMax) + geometry['0'].append(boundBox.YMax) + geometry['0'].append(boundBox.ZMax) + + boundBoxChild = v['child'].Shape.BoundBox + geometry['1'].append(boundBoxChild.XMax) + geometry['1'].append(boundBoxChild.YMax) + geometry['1'].append(boundBoxChild.ZMax) + meshParents = [] + + for el in v['parents']: + meshParents.append(MeshPartModel(el)) + joinMesh = JoinMeshModel(meshParents) + for el in meshParents: + el.remove() + import importOBJ + importOBJ.export(joinMesh.mesh, path + str(1) + '.obj') + joinMesh.remove() + importOBJ.export(v['child'], path + str(0) + '.obj') + FS.writeFile(json.dumps(geometry), path, 'translation.json') + + App.ActiveDocument.removeObject("Compound") + for el in copyLinks: + el.remove() + App.activeDocument().recompute() + inc += 1 + +def main(): + + env = FS.readJSON('./env.json') + env.get('cadDoc') + aspDir = env.get('aspDir') + sequences = FS.readJSON(env.get('sequences')).get('sequences') + App.openDocument(env.get('cadDoc')) + for sequencyNumber in range(len(sequences)): + FS.createFolder(aspDir + 'assemblys/') + mainFolder = aspDir + 'assemblys/' + str(sequencyNumber) + '/' + FS.createFolder(mainFolder) + for subSequenceNumber in range(len(sequences[sequencyNumber])): + if(subSequenceNumber != 0): + subFolder = aspDir + 'assemblys/' + \ + str(sequencyNumber) + '/' + str(subSequenceNumber) + '/' + + FS.createFolder(subFolder) + ExportAssemblyThemAllUseCase().call(path=subFolder,assemblys=sequences[sequencyNumber][0:subSequenceNumber+1]) + + App.closeDocument(App.ActiveDocument.Name) + freecadQTWindow = Gui.getMainWindow() + freecadQTWindow.close() +main() \ No newline at end of file diff --git a/insertion_vector_predicate/main.py b/insertion_vector_predicate/main.py index bec2e89..f41ca5b 100644 --- a/insertion_vector_predicate/main.py +++ b/insertion_vector_predicate/main.py @@ -1,4 +1,14 @@ -# Алгоритм генерации графа с помощью вычисления векторов вставки при разборке изделия +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 * @@ -9,18 +19,32 @@ 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 +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' -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/') - + class FS: def readJSON(path: str): @@ -43,6 +67,10 @@ class FS: 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) @@ -56,31 +84,44 @@ def filterModels(filterModels, filterModelsDescription): 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 + # 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') - # Коректировка пути до папки с генерацией ASP - if (aspDir == None): - args.print_helper() - if (aspDir[aspDir.__len__() - 1] != '/'): - aspDir += '/' - # Получение списка папок с .obj обьектами - assembles = FS.readFolder(aspDir + 'assembly') assemblyDirNormalize = [] - for el in assembles: - try: - # Пост обработка .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 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) @@ -113,13 +154,13 @@ def main(): try: 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, + planner_name='rrt', + step_size=None, + max_time=None, + seed=1, return_path=True, - simplify=args.simplify, - render=args.render + simplify=False, + render=False ) print(f'Status: {status}, planning time: {t_plan}') diff --git a/insertion_vector_predicate/requirements.txt b/insertion_vector_predicate/requirements.txt index 4b67e47..f52e972 100644 --- a/insertion_vector_predicate/requirements.txt +++ b/insertion_vector_predicate/requirements.txt @@ -1,2 +1,3 @@ spatialmath -scipy \ No newline at end of file +scipy +uuid \ No newline at end of file diff --git a/pddl/main.py b/pddl/main.py index 0856cf0..74cf50f 100644 --- a/pddl/main.py +++ b/pddl/main.py @@ -1,26 +1,30 @@ import argparse from helper.fs import FS -from src.model.asm4_structure import Asm4Structure +from src.model.robossembler_assets import RobossemblerAssets from src.usecases.assembly_to_pddl_use_case import AssemblyToPddlUseCase - - -# python3 main.py --stepStructurePath --outPath /Users/idontsudo/robo/pddl/out/ -# python3 main.py --stepStructurePath /Users/idontsudo/framework/asp/out/step-structure.json --outPath /Users/idontsudo/robo/pddl/out/ if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument('--stepStructurePath', help='json step by FreeCad') - parser.add_argument('--outPath', help='save pddl path') - args = parser.parse_args() - - if args.stepStructurePath == None or args.outPath == None: - parser.print_help() - - data = FS.readJSON(args.stepStructurePath) + parser.add_argument("--sequencesPath", help="sequences path") - assemblyToPddlUseCase = AssemblyToPddlUseCase.call(assembly=data,rootLabel=data[0]) - FS.writeFile(assemblyToPddlUseCase['problem'] ,args.outPath, 'problem.pddl') - FS.writeFile(assemblyToPddlUseCase['domain'] ,args.outPath, 'domain.pddl') - - \ No newline at end of file + parser.add_argument("--outPath", help="save pddl path") + parser.add_argument("--robossemblerDbPath", help="robossembler_db is require") + args = parser.parse_args() + + if ( + args.sequencesPath == None + or args.outPath == None + or args.robossemblerDbPath == None + ): + parser.print_help() + + data = FS.readJSON(args.sequencesPath) + robossemblerDb = FS.readJSON(args.robossemblerDbPath) + + assemblyToPddlUseCase = AssemblyToPddlUseCase.call( + assembly=data, robossemblerDb=robossemblerDb + ) + + FS.writeFile(assemblyToPddlUseCase["problem"], args.outPath, "problem.pddl") + FS.writeFile(assemblyToPddlUseCase["domain"], args.outPath, "domain.pddl") diff --git a/pddl/mocks/domain.txt b/pddl/mocks/domain.txt index 4f750fd..b040027 100644 --- a/pddl/mocks/domain.txt +++ b/pddl/mocks/domain.txt @@ -1,9 +1,7 @@ -;; Modified domain taken from -;; "Knowledge transfer in robot manipulation tasks" by Jacob O. Huckaby 2014 (define (domain robossembler) (:requirements :strips :typing :adl :fluents :durative-actions) (:types - printer workspace - zone + workspace - zone part arm assembly @@ -12,9 +10,29 @@ (:predicates (arm_available ?a - arm) (part_at ?p - part ?z - zone) - (printer_ready ?p - printer) (part_of ?part - part ?whole - assembly) (assembly_order ?prev ?next - assembly) (assembled ?whole - assembly ?z - zone) ) -);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; + + (:functions) + + + (:durative-action assemble + :parameters (?p - part ?prev ?next - assembly ?w - workspace ?arm - arm) + :duration (= ?duration 5) + :condition (and + (at start (assembled ?prev ?w)) + (at start (part_at ?p ?w)) + (at start (part_of ?p ?next)) + (at start (arm_available ?arm)) + (at start (assembly_order ?prev ?next)) + ) + :effect (and + (at start (not (arm_available ?arm))) + (at end (not (part_at ?p ?w))) + (at end (arm_available ?arm)) + (at end (assembled ?next ?w)) + ) + ) +) \ No newline at end of file diff --git a/pddl/mocks/problem.txt b/pddl/mocks/problem.txt new file mode 100644 index 0000000..3082fdc --- /dev/null +++ b/pddl/mocks/problem.txt @@ -0,0 +1,25 @@ +(define (problem robossembler-p1) + (:domain robossembler) + (:objects + ;; information from Scene + rasmt - arm + workspace1 - workspace + ;; information from CAD + ${types} + ) + (:init + ;; information from Scene + (assembled subasm0 workspace1) + ${part_at} + (arm_available rasmt) + + ;; information from CAD + ${assembled} + ) + (:goal + (and + ;; information from CAD + (assembled ${target} workspace1) + ) + ) +)-= \ No newline at end of file diff --git a/pddl/src/model/robossembler_assets.py b/pddl/src/model/robossembler_assets.py new file mode 100644 index 0000000..19b7e8b --- /dev/null +++ b/pddl/src/model/robossembler_assets.py @@ -0,0 +1,399 @@ +from dataclasses import dataclass +import os +from returns.result import Result, Success, Failure +from typing import Optional, Any, List, TypeVar, Callable, Type, cast +from enum import Enum + +from helper.fs import FS + + +T = TypeVar("T") +EnumT = TypeVar("EnumT", bound=Enum) + + +def from_float(x: Any) -> float: + assert isinstance(x, (float, int)) and not isinstance(x, bool) + return float(x) + + +def from_none(x: Any) -> Any: + return x + + +def from_union(fs, x): + for f in fs: + try: + return f(x) + except: + pass + assert False + + +def to_float(x: Any) -> float: + assert isinstance(x, float) + return x + + +def from_str(x: Any) -> str: + assert isinstance(x, str) + return x + + +def from_int(x: Any) -> int: + assert isinstance(x, int) and not isinstance(x, bool) + return x + + +def from_list(f: Callable[[Any], T], x: Any) -> List[T]: + assert isinstance(x, list) + return [f(y) for y in x] + + +def to_class(c: Type[T], x: Any) -> dict: + assert isinstance(x, c) + return cast(Any, x).to_dict() + + +def to_enum(c: Type[EnumT], x: Any) -> EnumT: + assert isinstance(x, c) + return x.value + + +@dataclass +class Model: + name: Optional[str] = None + id: Optional[str] = None + path: Optional[str] = None + + @staticmethod + def from_dict(obj: Any) -> "Model": + assert isinstance(obj, dict) + name = from_union([from_str, from_none], obj.get("name")) + id = from_union([from_str, from_none], obj.get("id")) + path = from_union([from_str, from_none], obj.get("path")) + return Model(name, id, path) + + def to_dict(self) -> dict: + result: dict = {} + if self.name is not None: + result["name"] = from_union([from_str, from_none], self.name) + if self.id is not None: + result["id"] = from_union([from_str, from_none], self.id) + if self.path is not None: + result["path"] = from_union([from_str, from_none], self.path) + return result + + +@dataclass +class Pose: + x: Optional[float] = None + y: Optional[float] = None + z: Optional[float] = None + roll: Optional[float] = None + pitch: Optional[float] = None + yaw: Optional[float] = None + + @staticmethod + def from_dict(obj: Any) -> "Pose": + assert isinstance(obj, dict) + x = from_union([from_float, from_none], obj.get("x")) + y = from_union([from_float, from_none], obj.get("y")) + z = from_union([from_float, from_none], obj.get("z")) + roll = from_union([from_float, from_none], obj.get("roll")) + pitch = from_union([from_float, from_none], obj.get("pitch")) + yaw = from_union([from_float, from_none], obj.get("yaw")) + return Pose(x, y, z, roll, pitch, yaw) + + def to_dict(self) -> dict: + result: dict = {} + if self.x is not None: + result["x"] = from_union([to_float, from_none], self.x) + if self.y is not None: + result["y"] = from_union([to_float, from_none], self.y) + if self.z is not None: + result["z"] = from_union([to_float, from_none], self.z) + if self.roll is not None: + result["roll"] = from_union([to_float, from_none], self.roll) + if self.pitch is not None: + result["pitch"] = from_union([to_float, from_none], self.pitch) + if self.yaw is not None: + result["yaw"] = from_union([to_float, from_none], self.yaw) + return result + + +class TypeEnum(Enum): + ASSET = "asset" + LIGHT = "light" + ROBOT = "robot" + + +@dataclass +class Instance: + model_name: Optional[str] = None + model_id: Optional[str] = None + id: Optional[str] = None + pose: Optional[Pose] = None + scale: Optional[int] = None + type: Optional[TypeEnum] = None + parent: Optional[str] = None + light_type: Optional[str] = None + intencity: Optional[int] = None + diffuse: Optional[List[float]] = None + spot_angle: Optional[int] = None + + @staticmethod + def from_dict(obj: Any) -> "Instance": + assert isinstance(obj, dict) + model_name = from_union([from_str, from_none], obj.get("model_name")) + model_id = from_union([from_str, from_none], obj.get("model_id")) + id = from_union([from_str, from_none], obj.get("id")) + pose = from_union([Pose.from_dict, from_none], obj.get("pose")) + scale = from_union([from_int, from_none], obj.get("scale")) + type = from_union([TypeEnum, from_none], obj.get("type")) + parent = from_union([from_str, from_none], obj.get("parent")) + light_type = from_union([from_str, from_none], obj.get("light_type")) + intencity = from_union([from_int, from_none], obj.get("intencity")) + diffuse = from_union( + [lambda x: from_list(from_float, x), from_none], obj.get("diffuse") + ) + spot_angle = from_union([from_int, from_none], obj.get("spot_angle")) + return Instance( + model_name, + model_id, + id, + pose, + scale, + type, + parent, + light_type, + intencity, + diffuse, + spot_angle, + ) + + def fromMappingInstanceAtModel( + self, models: List[Model] + ) -> "MappingInstanceAtModel": + for el in models: + if el.id == self.model_id: + return MappingInstanceAtModel(instance=self, model=el) + return Failure("not found model at {self.model_id} ") + + def to_dict(self) -> dict: + result: dict = {} + if self.model_name is not None: + result["model_name"] = from_union([from_str, from_none], self.model_name) + if self.model_id is not None: + result["model_id"] = from_union([from_str, from_none], self.model_id) + if self.id is not None: + result["id"] = from_union([from_str, from_none], self.id) + if self.pose is not None: + result["pose"] = from_union( + [lambda x: to_class(Pose, x), from_none], self.pose + ) + if self.scale is not None: + result["scale"] = from_union([from_int, from_none], self.scale) + if self.type is not None: + result["type"] = from_union( + [lambda x: to_enum(TypeEnum, x), from_none], self.type + ) + if self.parent is not None: + result["parent"] = from_union([from_str, from_none], self.parent) + if self.light_type is not None: + result["light_type"] = from_union([from_str, from_none], self.light_type) + if self.intencity is not None: + result["intencity"] = from_union([from_int, from_none], self.intencity) + if self.diffuse is not None: + result["diffuse"] = from_union( + [lambda x: from_list(to_float, x), from_none], self.diffuse + ) + if self.spot_angle is not None: + result["spot_angle"] = from_union([from_int, from_none], self.spot_angle) + return result + + +class BasePose: + def __init__(self, x: float, y: float, z: float, **kwargs): + self.x = x + self.y = y + self.z = z + + def toPose(self, sdfXmlMock: str): + return ( + sdfXmlMock.replace("{x}", str(self.x)) + .replace("{y}", str(self.y)) + .replace("{z}", str(self.z)) + ) + + +class MappingInstanceAtModel(BasePose): + instance: Instance + model: Model + + def __init__(self, instance: Instance, model: Model) -> None: + self.instance = instance + self.model = model + pass + + def toSDF(self): + pose = self.instance.pose + match self.instance.type: + case TypeEnum.ASSET: + mock = FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../mocks/model_include_sdf.xml" + ) + # mockPose = self.toPose(mock) + return ( + mock.replace("{name}", str(self.model.name)) + .replace("{x}", str(pose.x)) + .replace("{y}", str(pose.y)) + .replace("{z}", str(pose.z)) + .replace("{pitch}", str(pose.pitch)) + .replace("{yaw}", str(pose.yaw)) + .replace("{roll}", str(pose.roll)) + .replace("{uri}", str(self.model.path)) + ) + case TypeEnum.LIGHT: + pathMock = ( + os.path.dirname(os.path.realpath(__file__)) + + "/../mocks/light_sdf.xml" + ) + + return ( + FS.readFile(pathMock) + .replace("{x}", str(pose.x)) + .replace("{y}", str(pose.y)) + .replace("{z}", str(pose.z)) + .replace("{pitch}", str(pose.pitch)) + .replace("{yaw}", str(pose.yaw)) + .replace("{roll}", str(pose.roll)) + .replace("{type_light}", str(self.instance.light_type)) + .replace("{name_light}", str("132")) + .replace("{r}", self.instance.diffuse[0]) + .replace("{g}", self.instance.diffuse[1]) + .replace("{b}", self.instance.diffuse[2]) + .replace("{a}", self.instance.diffuse[3]) + ) + + +@dataclass +class Gravity: + x: Optional[int] = None + y: Optional[int] = None + z: Optional[float] = None + + @staticmethod + def from_dict(obj: Any) -> "Gravity": + assert isinstance(obj, dict) + x = from_union([from_int, from_none], obj.get("x")) + y = from_union([from_int, from_none], obj.get("y")) + z = from_union([from_float, from_none], obj.get("z")) + return Gravity(x, y, z) + + def to_dict(self) -> dict: + result: dict = {} + if self.x is not None: + result["x"] = from_union([from_int, from_none], self.x) + if self.y is not None: + result["y"] = from_union([from_int, from_none], self.y) + if self.z is not None: + result["z"] = from_union([to_float, from_none], self.z) + return result + + +@dataclass +class Physics: + engine_name: Optional[str] = None + gravity: Optional[Gravity] = None + + @staticmethod + def from_dict(obj: Any) -> "Physics": + assert isinstance(obj, dict) + engine_name = from_union([from_str, from_none], obj.get("engine_name")) + gravity = from_union([Gravity.from_dict, from_none], obj.get("gravity")) + return Physics(engine_name, gravity) + + def to_dict(self) -> dict: + result: dict = {} + if self.engine_name is not None: + result["engine_name"] = from_union([from_str, from_none], self.engine_name) + if self.gravity is not None: + result["gravity"] = from_union( + [lambda x: to_class(Gravity, x), from_none], self.gravity + ) + return result + + def toSDF(self) -> str: + pathMock = os.path.dirname(os.path.realpath(__file__)) + "/../mocks/world.xml" + gravity = self.gravity + + return ( + FS.readFile(pathMock) + .replace("{gravity_x}", str(gravity.x)) + .replace("{gravity_y}", str(gravity.y)) + .replace("{gravity_z}", str(gravity.z)) + .replace("{engine_type}", str(self.engine_name)) + ) + + +@dataclass +class RobossemblerAssets: + models: Optional[List[Model]] = None + instances: Optional[List[Instance]] = None + physics: Optional[Physics] = None + + @staticmethod + def from_dict(obj: Any) -> "RobossemblerAssets": + assert isinstance(obj, dict) + models = from_union( + [lambda x: from_list(Model.from_dict, x), from_none], obj.get("models") + ) + + instances = from_union( + [lambda x: from_list(Instance.from_dict, x), from_none], + obj.get("instances"), + ) + + physics = from_union([Physics.from_dict, from_none], obj.get("physics")) + return RobossemblerAssets(models, instances, physics) + + def to_dict(self) -> dict: + result: dict = {} + if self.models is not None: + result["models"] = from_union( + [lambda x: from_list(lambda x: to_class(Model, x), x), from_none], + self.models, + ) + if self.instances is not None: + result["instances"] = from_union( + [lambda x: from_list(lambda x: to_class(Instance, x), x), from_none], + self.instances, + ) + if self.physics is not None: + result["physics"] = from_union( + [lambda x: to_class(Physics, x), from_none], self.physics + ) + return result + + def _getAllAtType(self, type: TypeEnum) -> List[Instance]: + return list(filter(lambda x: x.type == type, self.instances)) + + def getAllRobotsName(self) -> List[MappingInstanceAtModel]: + return list(map(lambda el: el, self._getAllAtType(type=TypeEnum.ROBOT))) + + def getAllLightInstances(self) -> List[Instance]: + return list( + map( + lambda el: el.fromMappingInstanceAtModel(self.models), + self._getAllAtType(type=TypeEnum.LIGHT), + ) + ) + + def getAllAssetsInstanceAtModel(self) -> List[MappingInstanceAtModel]: + return list( + map( + lambda el: el.fromMappingInstanceAtModel(self.models), + self._getAllAtType(type=TypeEnum.ASSET), + ) + ) diff --git a/pddl/src/usecases/assembly_to_pddl_use_case.py b/pddl/src/usecases/assembly_to_pddl_use_case.py index a6ba8ef..e43211f 100644 --- a/pddl/src/usecases/assembly_to_pddl_use_case.py +++ b/pddl/src/usecases/assembly_to_pddl_use_case.py @@ -1,53 +1,86 @@ from typing import List from helper.fs import FS -from unified_planning.shortcuts import * -from unified_planning import * import os +from src.model.robossembler_assets import RobossemblerAssets + + +def doubleTagGenerate(instances: List[str], predicates: str, tag: str): + result = "" + for detail in instances: + result += "\n (" + predicates + " " + detail + " " + tag + ")" + "\n" + return result + + +def typeGenerator(types: list[str], typeName: str): + result = "" + for t in types: + result += t + " " + result += "- " + typeName + return result + + +def assemblyTagGenerate(sequences: list[str]): + result = "" + inc = 0 + for el in sequences: + result += doubleTagGenerate( + instances=[el], predicates="part_of", tag="subasm" + str(inc) + ) + inc += 1 + + inc = 0 + for el in sequences[1 : sequences.__len__()]: + result += doubleTagGenerate( + instances=["subasm" + str(inc)], + predicates="assembly_order", + tag="subasm" + str(inc + 1), + ) + inc += 1 + return result class AssemblyToPddlUseCase: - def call(assembly: List[str], rootLabel: str): - partType = UserType("part") - assemblyType = UserType('assembly') - - objectsPartPddl = [] - objectsAsmToPddl = [] - i = 0 - for el in assembly: - objectsPartPddl.append(Object(el, partType)) - - problem = Problem(rootLabel) - - for el in objectsPartPddl: - problem.add_object(el) - i = 0 - for el in objectsPartPddl: - problem.add_object(Object('subasm' + str(i), assemblyType)) - objectsAsmToPddl.append(Object('subasm' + str(i), assemblyType)) - i = i+1 - - connected = Fluent('part-of', BoolType(), - l_from=partType, l_to=assemblyType) - assemblyOrder = Fluent('assembly_order', BoolType(), - l_from=assemblyType, l_to=assemblyType) - i = 0 - for el in objectsPartPddl: - problem.set_initial_value(connected(el, objectsAsmToPddl[i]), True) - i = i+1 - goal = Fluent(rootLabel) - - problem.add_goal(connected(objectsPartPddl[objectsPartPddl.__len__( - ) - 1], objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]),) - - i = 0 - for el in objectsAsmToPddl: - if objectsAsmToPddl[i-1] != objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]: - problem.set_initial_value(assemblyOrder(objectsAsmToPddl[i-1], el), True) - i = i+1 - - problem.add_goal(assemblyOrder(objectsAsmToPddl[objectsAsmToPddl.__len__( - ) - 1], objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]),) + def call(assembly: List[str], robossemblerDb: RobossemblerAssets): + result = "" + robots = robossemblerDb.getAllRobotsName() + for robot in robots: + result += ( + FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../../mocks" + + "/problem.txt" + ) + .replace( + "${types}", + typeGenerator(assembly, "part") + + "\n " + + typeGenerator( + list( + map( + lambda x: "subasm" + str(x), + list(range(assembly.__len__())), + ) + ), + "assembly", + robot, + ), + ) + .replace( + "${assembled}", + assemblyTagGenerate(assembly), + ) + .replace( + "${part_at}", + doubleTagGenerate( + instances=assembly, predicates="part_at", tag="workspace1" + ), + ) + ).replace("${target}", "subasm" + str(assembly.__len__() - 1)) return { - "problem": unified_planning.io.PDDLWriter(problem).get_problem(), - 'domain': FS.readFile(os.path.dirname(os.path.realpath(__file__)) + '/../../mocks' + '/domain.txt'), + "problem": result, + "domain": FS.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../../mocks" + + "/domain.txt" + ), } diff --git a/pddl/unit.test.py b/pddl/unit.test.py index df08c98..2f0647d 100644 --- a/pddl/unit.test.py +++ b/pddl/unit.test.py @@ -1,26 +1,33 @@ import unittest from src.usecases.assembly_to_pddl_use_case import AssemblyToPddlUseCase from src.model.asm4_structure import Asm4Structure - + from helper.fs import FS import os -mock = FS.readJSON(os.path.dirname(os.path.realpath(__file__)) + '/mocks/step-mock.json') +mock = FS.readJSON( + os.path.dirname(os.path.realpath(__file__)) + "/mocks/step-mock.json" +) +# assembly навык - -assemblyToPddl = AssemblyToPddlUseCase.call(assembly=mock,rootLabel=mock[0]) +assemblyToPddl = AssemblyToPddlUseCase.call(assembly=mock, rootLabel=mock[0]) + - class TestStringMethods(unittest.TestCase): def test_problem(self): print(assemblyToPddl["problem"]) self.assertIsInstance(assemblyToPddl["problem"], str) + def test_domain(self): self.assertIsInstance(assemblyToPddl["domain"], str) - - -if __name__ == '__main__': + +if __name__ == "__main__": unittest.main() + + +# part_of определяет к какой подсборке относится деталь +# assembly определяет зоны +# diff --git a/robossembler_scene_builder/main.py b/robossembler_scene_builder/main.py new file mode 100644 index 0000000..70d8cba --- /dev/null +++ b/robossembler_scene_builder/main.py @@ -0,0 +1,97 @@ +from types import LambdaType, UnionType +from returns.pipeline import is_successful +from typing import List, TypeVar +from returns.result import Result, Success, Failure +import os +from model.robossembler_assets import ( + MappingInstanceAtModel, + RobossemblerAssets, + Instance, +) +import re +import pathlib +from repository.file_system import FileSystemRepository +from model.robossembler_assets import Physics +from argparse import ArgumentParser + + +T = TypeVar("T") + + +class JsonReaderAndModelMapperUseCase: + def call(path: str, model: T) -> Result[T, str]: + try: + if not re.search("^(.+)\/([^\/]+)$", path): + return Failure("path not valid") + if model.from_dict == None: + return Failure("Model is not have mapping method from_dict") + return Success(model.from_dict(FileSystemRepository.readJSON(path=path))) + except: + return Failure("JsonReaderAndModelMapperUseCase unknown error") + + +class MappingInstanceAtModelToSdfUseCase: + def call(instances: List[MappingInstanceAtModel]) -> Result[List[str], str]: + try: + return Success(list(map(lambda el: el.toSDF(), instances))) + except: + return Failure("MappingInstanceAtModelToSdfUseCase unknown error") + + +class MappingSdfWorldToPhysicsModelUseCase: + def call(physicModel: Physics) -> Result[List[str], str]: + try: + return Success(Physics.toSDF(physicModel)) + except: + return Failure("MappingInstanceAtModelToSdfUseCase unknown error") + + +class FormationOfTheSDFUseCase: + def call(worldTag: str, modelsTags: List[str], path: str) -> Result[bool, str]: + path = str(pathlib.Path(path).parent.resolve()) + "/" + if modelsTags == None: + return Failure("FormationOfTheSDFUseCase modelsTags is None") + if worldTag == None: + return Failure("FormationOfTheSDFUseCase worldTag is None") + + FileSystemRepository.writeFile( + data=worldTag.replace("{models}", "\n".join(modelsTags)), + filePath=path, + fileName="world.sdf", + ) + return Success(True) + + +def main(): + parser = ArgumentParser() + parser.add_argument("--path", help="need path .json") + args = parser.parse_args() + + if args.path == None: + parser.print_help() + return + path = args.path + jsonReaderAndModelMapperUseCase = JsonReaderAndModelMapperUseCase.call( + path=path, model=RobossemblerAssets + ) + + if not is_successful(jsonReaderAndModelMapperUseCase): + return + robossemblerAssets = jsonReaderAndModelMapperUseCase.value_or(None) + + instanceSdfModel = MappingInstanceAtModelToSdfUseCase.call( + instances=robossemblerAssets.getAllAssetsInstanceAtModel() + ) + + sdfWorld = MappingSdfWorldToPhysicsModelUseCase.call( + physicModel=robossemblerAssets.physics + ) + + FormationOfTheSDFUseCase.call( + worldTag=sdfWorld.value_or(None), + modelsTags=instanceSdfModel.value_or(None), + path=path, + ) + + +main() diff --git a/robossembler_scene_builder/mocks/light_sdf.xml b/robossembler_scene_builder/mocks/light_sdf.xml new file mode 100644 index 0000000..7e5aeee --- /dev/null +++ b/robossembler_scene_builder/mocks/light_sdf.xml @@ -0,0 +1,12 @@ + + {x} {y} {z} {roll} {pitch} {yaw} + {r} {g} {b} {a} + .1 .1 .1 1 + + 20 + 0.2 + 0.8 + 0.01 + + false + \ No newline at end of file diff --git a/robossembler_scene_builder/mocks/model_include_sdf.xml b/robossembler_scene_builder/mocks/model_include_sdf.xml new file mode 100644 index 0000000..e23a942 --- /dev/null +++ b/robossembler_scene_builder/mocks/model_include_sdf.xml @@ -0,0 +1,7 @@ + + {x} {y} {z} {roll} {pitch} {yaw} + + {name} + model://{uri} + + \ No newline at end of file diff --git a/robossembler_scene_builder/mocks/world.xml b/robossembler_scene_builder/mocks/world.xml new file mode 100644 index 0000000..7265866 --- /dev/null +++ b/robossembler_scene_builder/mocks/world.xml @@ -0,0 +1,105 @@ + + + + + 0.001 + 1.0 + 1000 + + + + + + + ogre2 + + {gravity_x} {gravity_y} {gravity_z} + + 6e-06 + 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + false + + + + + 3D View + false + docked + + ogre2 + scene + 1.0 1.0 1.0 + 0.4 0.6 1.0 + 3.3 2.8 2.8 0 0.5 -2.4 + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + {models} + \ No newline at end of file diff --git a/robossembler_scene_builder/model/robossembler_assets.py b/robossembler_scene_builder/model/robossembler_assets.py new file mode 100644 index 0000000..c6cf484 --- /dev/null +++ b/robossembler_scene_builder/model/robossembler_assets.py @@ -0,0 +1,394 @@ +from dataclasses import dataclass +import os +from returns.result import Result, Success, Failure +from typing import Optional, Any, List, TypeVar, Callable, Type, cast +from enum import Enum + +from repository.file_system import FileSystemRepository + +T = TypeVar("T") +EnumT = TypeVar("EnumT", bound=Enum) + + +def from_float(x: Any) -> float: + assert isinstance(x, (float, int)) and not isinstance(x, bool) + return float(x) + + +def from_none(x: Any) -> Any: + return x + + +def from_union(fs, x): + for f in fs: + try: + return f(x) + except: + pass + assert False + + +def to_float(x: Any) -> float: + assert isinstance(x, float) + return x + + +def from_str(x: Any) -> str: + assert isinstance(x, str) + return x + + +def from_int(x: Any) -> int: + assert isinstance(x, int) and not isinstance(x, bool) + return x + + +def from_list(f: Callable[[Any], T], x: Any) -> List[T]: + assert isinstance(x, list) + return [f(y) for y in x] + + +def to_class(c: Type[T], x: Any) -> dict: + assert isinstance(x, c) + return cast(Any, x).to_dict() + + +def to_enum(c: Type[EnumT], x: Any) -> EnumT: + assert isinstance(x, c) + return x.value + + +@dataclass +class Model: + name: Optional[str] = None + id: Optional[str] = None + path: Optional[str] = None + + @staticmethod + def from_dict(obj: Any) -> "Model": + assert isinstance(obj, dict) + name = from_union([from_str, from_none], obj.get("name")) + id = from_union([from_str, from_none], obj.get("id")) + path = from_union([from_str, from_none], obj.get("path")) + return Model(name, id, path) + + def to_dict(self) -> dict: + result: dict = {} + if self.name is not None: + result["name"] = from_union([from_str, from_none], self.name) + if self.id is not None: + result["id"] = from_union([from_str, from_none], self.id) + if self.path is not None: + result["path"] = from_union([from_str, from_none], self.path) + return result + + +@dataclass +class Pose: + x: Optional[float] = None + y: Optional[float] = None + z: Optional[float] = None + roll: Optional[float] = None + pitch: Optional[float] = None + yaw: Optional[float] = None + + @staticmethod + def from_dict(obj: Any) -> "Pose": + assert isinstance(obj, dict) + x = from_union([from_float, from_none], obj.get("x")) + y = from_union([from_float, from_none], obj.get("y")) + z = from_union([from_float, from_none], obj.get("z")) + roll = from_union([from_float, from_none], obj.get("roll")) + pitch = from_union([from_float, from_none], obj.get("pitch")) + yaw = from_union([from_float, from_none], obj.get("yaw")) + return Pose(x, y, z, roll, pitch, yaw) + + def to_dict(self) -> dict: + result: dict = {} + if self.x is not None: + result["x"] = from_union([to_float, from_none], self.x) + if self.y is not None: + result["y"] = from_union([to_float, from_none], self.y) + if self.z is not None: + result["z"] = from_union([to_float, from_none], self.z) + if self.roll is not None: + result["roll"] = from_union([to_float, from_none], self.roll) + if self.pitch is not None: + result["pitch"] = from_union([to_float, from_none], self.pitch) + if self.yaw is not None: + result["yaw"] = from_union([to_float, from_none], self.yaw) + return result + + +class TypeEnum(Enum): + ASSET = "asset" + LIGHT = "light" + + +@dataclass +class Instance: + model_name: Optional[str] = None + model_id: Optional[str] = None + id: Optional[str] = None + pose: Optional[Pose] = None + scale: Optional[int] = None + type: Optional[TypeEnum] = None + parent: Optional[str] = None + light_type: Optional[str] = None + intencity: Optional[int] = None + diffuse: Optional[List[float]] = None + spot_angle: Optional[int] = None + + @staticmethod + def from_dict(obj: Any) -> "Instance": + assert isinstance(obj, dict) + model_name = from_union([from_str, from_none], obj.get("model_name")) + model_id = from_union([from_str, from_none], obj.get("model_id")) + id = from_union([from_str, from_none], obj.get("id")) + pose = from_union([Pose.from_dict, from_none], obj.get("pose")) + scale = from_union([from_int, from_none], obj.get("scale")) + type = from_union([TypeEnum, from_none], obj.get("type")) + parent = from_union([from_str, from_none], obj.get("parent")) + light_type = from_union([from_str, from_none], obj.get("light_type")) + intencity = from_union([from_int, from_none], obj.get("intencity")) + diffuse = from_union( + [lambda x: from_list(from_float, x), from_none], obj.get("diffuse") + ) + spot_angle = from_union([from_int, from_none], obj.get("spot_angle")) + return Instance( + model_name, + model_id, + id, + pose, + scale, + type, + parent, + light_type, + intencity, + diffuse, + spot_angle, + ) + + def fromMappingInstanceAtModel( + self, models: List[Model] + ) -> "MappingInstanceAtModel": + for el in models: + if el.id == self.model_id: + return MappingInstanceAtModel(instance=self, model=el) + return Failure("not found model at {self.model_id} ") + + def to_dict(self) -> dict: + result: dict = {} + if self.model_name is not None: + result["model_name"] = from_union([from_str, from_none], self.model_name) + if self.model_id is not None: + result["model_id"] = from_union([from_str, from_none], self.model_id) + if self.id is not None: + result["id"] = from_union([from_str, from_none], self.id) + if self.pose is not None: + result["pose"] = from_union( + [lambda x: to_class(Pose, x), from_none], self.pose + ) + if self.scale is not None: + result["scale"] = from_union([from_int, from_none], self.scale) + if self.type is not None: + result["type"] = from_union( + [lambda x: to_enum(TypeEnum, x), from_none], self.type + ) + if self.parent is not None: + result["parent"] = from_union([from_str, from_none], self.parent) + if self.light_type is not None: + result["light_type"] = from_union([from_str, from_none], self.light_type) + if self.intencity is not None: + result["intencity"] = from_union([from_int, from_none], self.intencity) + if self.diffuse is not None: + result["diffuse"] = from_union( + [lambda x: from_list(to_float, x), from_none], self.diffuse + ) + if self.spot_angle is not None: + result["spot_angle"] = from_union([from_int, from_none], self.spot_angle) + return result + + +class BasePose: + def __init__(self, x: float, y: float, z: float, **kwargs): + self.x = x + self.y = y + self.z = z + + def toPose(self, sdfXmlMock: str): + return ( + sdfXmlMock.replace("{x}", str(self.x)) + .replace("{y}", str(self.y)) + .replace("{z}", str(self.z)) + ) + + +class MappingInstanceAtModel(BasePose): + instance: Instance + model: Model + + def __init__(self, instance: Instance, model: Model) -> None: + self.instance = instance + self.model = model + pass + + def toSDF(self): + pose = self.instance.pose + match self.instance.type: + case TypeEnum.ASSET: + mock = FileSystemRepository.readFile( + os.path.dirname(os.path.realpath(__file__)) + + "/../mocks/model_include_sdf.xml" + ) + # mockPose = self.toPose(mock) + return ( + mock.replace("{name}", str(self.model.name)) + .replace("{x}", str(pose.x)) + .replace("{y}", str(pose.y)) + .replace("{z}", str(pose.z)) + .replace("{pitch}", str(pose.pitch)) + .replace("{yaw}", str(pose.yaw)) + .replace("{roll}", str(pose.roll)) + .replace("{uri}", str(self.model.path)) + ) + case TypeEnum.LIGHT: + pathMock = ( + os.path.dirname(os.path.realpath(__file__)) + + "/../mocks/light_sdf.xml" + ) + + return ( + FileSystemRepository.readFile(pathMock) + .replace("{x}", str(pose.x)) + .replace("{y}", str(pose.y)) + .replace("{z}", str(pose.z)) + .replace("{pitch}", str(pose.pitch)) + .replace("{yaw}", str(pose.yaw)) + .replace("{roll}", str(pose.roll)) + .replace("{type_light}", str(self.instance.light_type)) + .replace("{name_light}", str("132")) + .replace("{r}", self.instance.diffuse[0]) + .replace("{g}", self.instance.diffuse[1]) + .replace("{b}", self.instance.diffuse[2]) + .replace("{a}", self.instance.diffuse[3]) + ) + + +@dataclass +class Gravity: + x: Optional[int] = None + y: Optional[int] = None + z: Optional[float] = None + + @staticmethod + def from_dict(obj: Any) -> "Gravity": + assert isinstance(obj, dict) + x = from_union([from_int, from_none], obj.get("x")) + y = from_union([from_int, from_none], obj.get("y")) + z = from_union([from_float, from_none], obj.get("z")) + return Gravity(x, y, z) + + def to_dict(self) -> dict: + result: dict = {} + if self.x is not None: + result["x"] = from_union([from_int, from_none], self.x) + if self.y is not None: + result["y"] = from_union([from_int, from_none], self.y) + if self.z is not None: + result["z"] = from_union([to_float, from_none], self.z) + return result + + +@dataclass +class Physics: + engine_name: Optional[str] = None + gravity: Optional[Gravity] = None + + @staticmethod + def from_dict(obj: Any) -> "Physics": + assert isinstance(obj, dict) + engine_name = from_union([from_str, from_none], obj.get("engine_name")) + gravity = from_union([Gravity.from_dict, from_none], obj.get("gravity")) + return Physics(engine_name, gravity) + + def to_dict(self) -> dict: + result: dict = {} + if self.engine_name is not None: + result["engine_name"] = from_union([from_str, from_none], self.engine_name) + if self.gravity is not None: + result["gravity"] = from_union( + [lambda x: to_class(Gravity, x), from_none], self.gravity + ) + return result + + def toSDF(self) -> str: + pathMock = os.path.dirname(os.path.realpath(__file__)) + "/../mocks/world.xml" + gravity = self.gravity + + return ( + FileSystemRepository.readFile(pathMock) + .replace("{gravity_x}", str(gravity.x)) + .replace("{gravity_y}", str(gravity.y)) + .replace("{gravity_z}", str(gravity.z)) + .replace("{engine_type}", str(self.engine_name)) + ) + + +@dataclass +class RobossemblerAssets: + models: Optional[List[Model]] = None + instances: Optional[List[Instance]] = None + physics: Optional[Physics] = None + + @staticmethod + def from_dict(obj: Any) -> "RobossemblerAssets": + assert isinstance(obj, dict) + models = from_union( + [lambda x: from_list(Model.from_dict, x), from_none], obj.get("models") + ) + + instances = from_union( + [lambda x: from_list(Instance.from_dict, x), from_none], + obj.get("instances"), + ) + + physics = from_union([Physics.from_dict, from_none], obj.get("physics")) + return RobossemblerAssets(models, instances, physics) + + def to_dict(self) -> dict: + result: dict = {} + if self.models is not None: + result["models"] = from_union( + [lambda x: from_list(lambda x: to_class(Model, x), x), from_none], + self.models, + ) + if self.instances is not None: + result["instances"] = from_union( + [lambda x: from_list(lambda x: to_class(Instance, x), x), from_none], + self.instances, + ) + if self.physics is not None: + result["physics"] = from_union( + [lambda x: to_class(Physics, x), from_none], self.physics + ) + return result + + def _getAllAtType(self, type: TypeEnum) -> List[Instance]: + return list(filter(lambda x: x.type == type, self.instances)) + + def getAllLightInstances(self) -> List[Instance]: + return list( + map( + lambda el: el.fromMappingInstanceAtModel(self.models), + self._getAllAtType(type=TypeEnum.LIGHT), + ) + ) + + def getAllAssetsInstanceAtModel(self) -> List[MappingInstanceAtModel]: + return list( + map( + lambda el: el.fromMappingInstanceAtModel(self.models), + self._getAllAtType(type=TypeEnum.ASSET), + ) + ) diff --git a/robossembler_scene_builder/repository/file_system.py b/robossembler_scene_builder/repository/file_system.py new file mode 100644 index 0000000..4f8ca26 --- /dev/null +++ b/robossembler_scene_builder/repository/file_system.py @@ -0,0 +1,25 @@ +import json +import os + + +class FileSystemRepository: + 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", encoding="utf-8", errors="ignore") + f.write(data) + f.close() + + def readFile(path: str): + return open(path).read() + + def readFilesTypeFolder(pathFolder: str, fileType=".json"): + filesJson = list( + filter( + lambda x: x[-fileType.__len__() :] == fileType, os.listdir(pathFolder) + ) + ) + return filesJson diff --git a/stability_process_predicate/main.py b/stability_process_predicate/main.py index 89ab934..ac8741f 100644 --- a/stability_process_predicate/main.py +++ b/stability_process_predicate/main.py @@ -1,12 +1,14 @@ import argparse from usecases.stability_check_usecase import StabilityCheckUseCase -# python3 main.py --aspPath /Users/idontsudo/framework/asp/out +# python3 main.py --aspPath /Users/idontsudo/Desktop/asp-example/ + def main(): parser = argparse.ArgumentParser() - parser.add_argument('--aspPath', help='asp folder generation path') + parser.add_argument("--aspPath", help="asp folder generation path") args = parser.parse_args() StabilityCheckUseCase().call(args.aspPath) + + main() - \ No newline at end of file diff --git a/stability_process_predicate/usecases/stability_check_usecase.py b/stability_process_predicate/usecases/stability_check_usecase.py index da35575..b4c1931 100644 --- a/stability_process_predicate/usecases/stability_check_usecase.py +++ b/stability_process_predicate/usecases/stability_check_usecase.py @@ -53,7 +53,7 @@ class Coords: self.z = z @staticmethod - def from_dict(obj: Any) -> 'Coords': + def from_dict(obj: Any) -> "Coords": assert isinstance(obj, dict) x = from_float(obj.get("x")) y = from_float(obj.get("y")) @@ -79,7 +79,7 @@ class SimulatorStabilityResultModel: self.position = position @staticmethod - def from_dict(obj: Any) -> 'SimulatorStabilityResultModel': + def from_dict(obj: Any) -> "SimulatorStabilityResultModel": assert isinstance(obj, dict) id = from_str(obj.get("id")) quaternion = Coords.from_dict(obj.get("quaternion")) @@ -103,90 +103,127 @@ def SimulatorStabilityModeltodict(x: List[SimulatorStabilityResultModel]) -> Any class StabilityCheckUseCase: - def urdfLoader(self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str]): - + def urdfLoader( + self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str] + ): urdfs = [] for assemblyCount in range(len(assembly)): - urdf = urdfGeneration.get(assembly[assemblyCount]) - file_to_open = outPath + '/sdf-generation/' + \ - str(assemblyCount) + '.urdf' - f = open(file_to_open, 'w', encoding='utf-8', - errors='ignore') + file_to_open = outPath + "/generation/" + str(assemblyCount) + ".urdf" + f = open(file_to_open, "w", encoding="utf-8", errors="ignore") f.write(urdf) f.close() urdfs.append(os.path.abspath(f.name)) return urdfs - def executeSimulation(self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str], duration: int) -> list['SimulatorStabilityResultModel']: - + def executeSimulation( + self, + assembly: list[str], + outPath: str, + urdfGeneration: dict[str:str], + duration: int, + ) -> list["SimulatorStabilityResultModel"]: p.connect(p.DIRECT) p.setGravity(0, 0, -10) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") resultCoords = [] - urdfs = self.urdfLoader(assembly=assembly, - urdfGeneration=urdfGeneration, outPath=outPath) + urdfs = self.urdfLoader( + assembly=assembly, urdfGeneration=urdfGeneration, outPath=outPath + ) bulletIds = [] for el in urdfs: id = p.loadURDF(el) bulletIds.append(id) for i in range(duration): - if (i + 200 == duration): + if i + 200 == duration: inc = 0 for bulletUUID in bulletIds: pos, rot = p.getBasePositionAndOrientation(bulletUUID) - resultCoords.append(SimulatorStabilityResultModel(id=assembly[inc], quaternion=Coords( - x=rot[0], y=rot[1], z=rot[2]), position=Coords(x=pos[0], y=pos[1], z=pos[2]))) + resultCoords.append( + SimulatorStabilityResultModel( + id=assembly[inc], + quaternion=Coords(x=rot[0], y=rot[1], z=rot[2]), + position=Coords(x=pos[0], y=pos[1], z=pos[2]), + ) + ) p.removeBody(bulletUUID) inc += 1 p.stepSimulation() - time.sleep(1./240.) + time.sleep(1.0 / 240.0) return resultCoords def call(self, aspPath: str): try: assemblyFolder = aspPath assemblesStructures = json.loads( - (open(assemblyFolder + 'sequences.json')).read()).get('sequences') + (open(assemblyFolder + "sequences.json")).read() + ).get("sequences") + tasks = len(assemblesStructures) * len(assemblesStructures[0]) taskCounter = 0 urdfGeneration = json.loads( - (open(assemblyFolder + 'sdf-generation/urdf-generation.json')).read()) + (open(assemblyFolder + "generation/urdf-generation.json")).read() + ) for activeAssemblyNumber in range(len(assemblesStructures)): - pathSaveResultAssemblyFolder = aspPath + 'stability' + \ - '/' + str(activeAssemblyNumber + 1) + '/' + pathSaveResultAssemblyFolder = ( + aspPath + "stability" + "/" + str(activeAssemblyNumber + 1) + "/" + ) if not os.path.exists(pathSaveResultAssemblyFolder): os.makedirs(pathSaveResultAssemblyFolder) - for subAssemblyNumber in range(len(assemblesStructures[activeAssemblyNumber])): + for subAssemblyNumber in range( + len(assemblesStructures[activeAssemblyNumber]) + ): taskCounter += 1 - subAssembly = assemblesStructures[activeAssemblyNumber][0:subAssemblyNumber+1] - asm = [] - for el in subAssembly: - asm.append(el) + subAssembly = assemblesStructures[activeAssemblyNumber][ + 0 : subAssemblyNumber + 1 + ] + print(subAssembly) - resultSimulationStates = self.executeSimulation( - assembly=asm, outPath=aspPath, urdfGeneration=urdfGeneration, duration=1000) + if subAssembly == [ + "disk_top", + "disk_middel", + ]: + asm = [] + for el in subAssembly: + asm.append(el) - pathSaveResultSubAssemblyFolder = aspPath + 'stability' + '/' + \ - str(activeAssemblyNumber + 1) + \ - '/' + str(subAssemblyNumber) + '/' - if not os.path.exists(pathSaveResultSubAssemblyFolder): - os.makedirs(pathSaveResultSubAssemblyFolder) - results = {} - for state in resultSimulationStates: - results[state.id] = state.to_dict() - f = open(pathSaveResultSubAssemblyFolder + '/' + - 'motion_result.json', 'w', encoding='utf-8', errors='ignore') - f.write(json.dumps(results, - ensure_ascii=False, indent=4)) - f.close() - percentageOfCompletion = taskCounter / tasks * 100 - print('process complete: ' + - str(percentageOfCompletion) + '%') + resultSimulationStates = self.executeSimulation( + assembly=asm, + outPath=aspPath, + urdfGeneration=urdfGeneration, + duration=1000, + ) + + pathSaveResultSubAssemblyFolder = ( + aspPath + + "stability" + + "/" + + str(activeAssemblyNumber + 1) + + "/" + + str(subAssemblyNumber) + + "/" + ) + if not os.path.exists(pathSaveResultSubAssemblyFolder): + os.makedirs(pathSaveResultSubAssemblyFolder) + results = {} + for state in resultSimulationStates: + results[state.id] = state.to_dict() + f = open( + pathSaveResultSubAssemblyFolder + + "/" + + "motion_result.json", + "w", + encoding="utf-8", + errors="ignore", + ) + f.write(json.dumps(results, ensure_ascii=False, indent=4)) + f.close() + percentageOfCompletion = taskCounter / tasks * 100 + print("process complete: " + str(percentageOfCompletion) + "%") except Exception as e: print(e)