diff --git a/.gitignore b/.gitignore index 3390209..3332cf1 100644 --- a/.gitignore +++ b/.gitignore @@ -112,3 +112,6 @@ install_plugin_cad.sh *# .#* \#*\# +Cube3 +sdf-generation +p.json \ No newline at end of file diff --git a/cg/freecad/Frames/usecases/export_usecase.py b/cg/freecad/Frames/usecases/export_usecase.py index 519982a..bce09c6 100644 --- a/cg/freecad/Frames/usecases/export_usecase.py +++ b/cg/freecad/Frames/usecases/export_usecase.py @@ -1,15 +1,15 @@ -import importDAE +# import importDAE import FreeCAD as App from model.files_generator import FolderGenerator from helper.is_solid import is_object_solid - +import Mesh class ExportUseCase: def call(path): meshes = {} for el in App.ActiveDocument.Objects: if (is_object_solid(el)): - importDAE.export([el], path + '/' + FolderGenerator.SDF.value + + Mesh.export([el], path + '/' + FolderGenerator.SDF.value + '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae') meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \ '/' + el.Label + '.dae' diff --git a/sdf/helper/fs.py b/sdf/helper/fs.py index a529801..0cdfb23 100644 --- a/sdf/helper/fs.py +++ b/sdf/helper/fs.py @@ -1,5 +1,6 @@ import os import json +import typing class FS: @@ -21,3 +22,14 @@ class FS: filter(lambda x: x[-fileType.__len__():] == fileType, os.listdir(pathFolder))) return filesJson + +def listGetFirstValue(iterable, default=False, pred=None): + return next(filter(pred, iterable), default) + + +def filterModels(filterModels, filterModelsDescription: list[str]): + models = [] + for el in filterModelsDescription: + models.append(listGetFirstValue( + filterModels, None, lambda x: x.name == el)) + return models diff --git a/sdf/main.py b/sdf/main.py index e4ee14f..893c97d 100644 --- a/sdf/main.py +++ b/sdf/main.py @@ -1,71 +1,56 @@ import argparse import shutil -from distutils.dir_util import copy_tree -import asyncio from helper.fs import FS -from src.usecases.generate_world import SdfGenerateWorldUseCase -from src.model.sdf_geometry import SdfGeometryModel +from src.usecases.stability_check_usecase import StabilityCheckUseCase +from src.usecases.urdf_sub_assembly_usecase import UrdfSubAssemblyUseCase +from src.usecases.sdf_generate_world_usecase import SdfGenerateWorldUseCase +from src.model.sdf_geometry import GeometryModel from src.usecases.sdf_sub_assembly_usecase import SdfSubAssemblyUseCase import os -import typing -import xmlformatter - -# python3 main.py --generationFolder /Users/idontsudo/robo/Cube3/ --outPath /Users/idontsudo/robo/ --world true +# python3 main.py --generationFolder /Users/idontsudo/robo/Cube3/ --outPath /Users/idontsudo/robo/ --world true --format 'urdf' --stabilityCheck 'true' +# python3 main.py --generationFolder /Users/idontsudo/robo/Cube3/ --outPath /Users/idontsudo/robo/ --world true --format 'sdf' + 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('--stabilityCheck', + help='do i need to check the stability?') 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') - sdfGeometryModels: list[SdfGeometryModel] = [] + geometryModels: list[GeometryModel] = [] for el in geometryFiles: - sdfGeometryModels.append(SdfGeometryModel.from_dict( + geometryModels.append(GeometryModel.from_dict( FS.readJSON(args.generationFolder + '/assets/' + el))) - sdfSubAssemblyUseCase = SdfSubAssemblyUseCase().call( - sdfGeometryModels, assemblyStructure,) - if os.path.exists(outPath + 'sdf-generation/'): shutil.rmtree(path=outPath + 'sdf-generation/') - - copy_tree(args.generationFolder + 'sdf/', outPath + 'sdf-generation/') - dirPath = outPath + 'sdf-generation/' - for el in sdfGeometryModels: - path = dirPath + el.name + '/' - os.makedirs(path) - FS.writeFile(data=el.toSDF(), filePath=path, - fileName='/model' + '.sdf') - FS.writeFile(data=FS.readFile(os.path.dirname(os.path.realpath(__file__)) - + '/mocks/sdf/model.config'), filePath=path, fileName='/model' + '.config') - - - if(args.world == None): - for key, v in sdfSubAssemblyUseCase.items(): - FS.writeFile(data=v['assembly'], filePath=dirPath, - fileName='/' + key + '.sdf') - - else: - for key, v in sdfSubAssemblyUseCase.items(): - FS.writeFile(data=SdfGenerateWorldUseCase.call(v['assembly']), filePath=dirPath, - fileName='/' + key + '.sdf') - formatter = xmlformatter.Formatter(indent="1", indent_char="\t", encoding_output="ISO-8859-1", preserve=["literal"]) - - files = FS.readFilesTypeFolder(outPath + 'sdf-generation/', fileType= '.sdf') - for el in files: - - FS.writeFile(data=str(formatter.format_file(outPath + 'sdf-generation/' + el) , 'utf-8'), filePath=outPath + 'sdf-generation/', fileName=el) - - + if (args.format == 'sdf'): + SdfSubAssemblyUseCase().call( + geometryModels=geometryModels, assembly=assemblyStructure, + world=args.world, + generationFolder=args.generationFolder, + outPath=args.outPath + ) + if (args.format == 'urdf' and args.stabilityCheck != None): + UrdfSubAssemblyUseCase().call( + geometryModels=geometryModels, assembly=assemblyStructure, + world=args.world, + generationFolder=args.generationFolder, + outPath=args.outPath + ) + StabilityCheckUseCase().call( + args.outPath + ) diff --git a/sdf/mocks/urdf/model.urdf b/sdf/mocks/urdf/model.urdf new file mode 100644 index 0000000..8c210af --- /dev/null +++ b/sdf/mocks/urdf/model.urdf @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/sdf/src/model/asm.py b/sdf/src/model/asm.py new file mode 100644 index 0000000..db07cb3 --- /dev/null +++ b/sdf/src/model/asm.py @@ -0,0 +1,16 @@ +from distutils.dir_util import copy_tree +from src.model.enum import Enum + +class Assembly: + def generateSubAssembly(self, assembly: list[str]): + asm = {} + inc = 0 + for el in assembly: + asm[str("asm" + str(inc))] = { + "part": el, + "assembly": assembly[0:inc], + } + inc += 1 + return asm + def copy(self,generationFolder,format,outPath ): + copy_tree(generationFolder + format, outPath + Enum.folderPath) \ No newline at end of file diff --git a/sdf/src/model/enum.py b/sdf/src/model/enum.py new file mode 100644 index 0000000..88f24f8 --- /dev/null +++ b/sdf/src/model/enum.py @@ -0,0 +1,2 @@ +class Enum: + folderPath = 'sdf-generation/'; diff --git a/sdf/src/model/sdf_geometry.py b/sdf/src/model/sdf_geometry.py index 866a7ff..055b859 100644 --- a/sdf/src/model/sdf_geometry.py +++ b/sdf/src/model/sdf_geometry.py @@ -30,7 +30,7 @@ def to_class(c, x): return x.to_dict() -class SdfGeometryModel: +class GeometryModel: def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction): self.name = name self.ixx = ixx @@ -71,7 +71,7 @@ class SdfGeometryModel: link = from_union([from_str, from_none], obj.get('link')) friction = from_union([from_str, from_none], obj.get("friction")) - return SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction) + return GeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction) def to_dict(self): result = {} @@ -116,46 +116,50 @@ class SdfGeometryModel: 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) + + '/../../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) - 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) + + '/../../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) - - def generateSDFatJoinFixed(self, sdfModels: list['SdfGeometryModel']): + + def generateSDFatJoinFixed(self, sdfModels: list['GeometryModel']): sdf = '\n\n' - sdf+= ' \n' + sdf += ' \n' sdf += " 0 0 0 0 0 0\n" - sdf+= " \n" - + sdf += " \n" + link = sdf + self.includeLink(pose=True) if sdfModels.__len__() == 0: return link endTagLinkInc = link.__len__() beginSDF = link[0: endTagLinkInc] - sdfJoin = beginSDF + '\n' - + for el in sdfModels: if el.name != self.name: sdfJoin += el.includeLink(pose=True) + '\n' - + 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' + parent=self.name, child=el.name, modelAt=el).toSDF() + '\n' sdfJoin += endSDF sdfJoin += '' return sdfJoin - \ No newline at end of file + + 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) + diff --git a/sdf/src/usecases/formatter_usecase.py b/sdf/src/usecases/formatter_usecase.py new file mode 100644 index 0000000..fa0de3f --- /dev/null +++ b/sdf/src/usecases/formatter_usecase.py @@ -0,0 +1,15 @@ +from src.model.enum import Enum +import xmlformatter +from helper.fs import FS + + +class FormatterUseCase: + def call(outPath: str, format: str): + formatter = xmlformatter.Formatter( + indent="1", indent_char="\t", encoding_output="ISO-8859-1", preserve=["literal"]) + + files = FS.readFilesTypeFolder( + outPath + Enum.folderPath, fileType=format) + for el in files: + FS.writeFile(data=str(formatter.format_file(outPath + Enum.folderPath + el), + 'utf-8'), filePath=outPath + Enum.folderPath, fileName=el) diff --git a/sdf/src/usecases/sdf_generate_world_usecase.py b/sdf/src/usecases/sdf_generate_world_usecase.py new file mode 100644 index 0000000..96b842e --- /dev/null +++ b/sdf/src/usecases/sdf_generate_world_usecase.py @@ -0,0 +1,12 @@ +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('