Support "Assemble them all" trajectory generation from CAD

asp/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)

asp/src/usecases/generate_world.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('</world') - 1]
+        endWorld = world[world.find('</world') - 1: world.__len__()]
+        
+        
+        return beginWorld + assembly + endWorld

asp/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('</world') - 1]
+        endWorld = world[world.find('</world') - 1: world.__len__()]
+        
+        
+        return beginWorld + assembly + endWorld

asp/src/usecases/sdf_sub_assembly_usecase.py

@@ -0,0 +1,57 @@
+import os
+from typing import Optional
+from helper.fs import FS
+from helper.fs import filterModels, listGetFirstValue
+from src.model.asm import Assembly
+from src.model.enum import Enum
+from src.usecases.formatter_usecase import FormatterUseCase
+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'
+
+ 
+
+
+class SdfSubAssemblyUseCase(Assembly):
+
+    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:
+
+                model: Optional[GeometryModel] = listGetFirstValue(
+                    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()}
+
+        self.copy(generationFolder=
+                  generationFolder, format='/sdf', outPath=outPath)
+        dirPath = outPath + Enum.folderPath
+        for el in geometryModels:
+            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')
+
+        for key, v in asm.items():
+            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)
+
+        FormatterUseCase.call(outPath=outPath, format=SDF_FILE_FORMAT)

asp/src/usecases/stability_check_usecase.py

@@ -0,0 +1,36 @@
+import numpy as np
+import pybullet as p
+import time
+import pybullet_data
+from helper.fs import FS
+from src.usecases.urdf_sub_assembly_usecase import URDF_GENERATOR_FILE
+import json
+
+from src.model.enum import Enum
+
+
+class StabilityCheckUseCase:
+    def call(self, outPath: str):
+        dirPath = outPath + Enum.folderPath
+        DURATION = 10000
+        asm = json.loads(FS.readFile(dirPath + URDF_GENERATOR_FILE))
+        inc = 0
+        for el in asm['asm2']:
+            FS.writeFile(data=el, filePath=dirPath,
+                         fileName=str(inc) + '.urdf')
+            inc += 1
+        assemblyURDFS = list(
+            map(lambda el: dirPath+el, FS.readFilesTypeFolder(dirPath, '.urdf')))
+        physicsClient = p.connect(p.GUI)
+
+        p.setGravity(0, 0, -10)
+        
+        for el in assemblyURDFS:
+            p.loadURDF(el)
+      
+
+        for i in range(DURATION):
+            p.stepSimulation()
+            time.sleep(1./240.)
+           
+        p.disconnect()

asp/src/usecases/urdf_sub_assembly_usecase.py

@@ -0,0 +1,42 @@
+from typing import Optional
+from helper.fs import FS
+from src.model.enum import Enum
+from src.model.asm import Assembly
+from src.model.sdf_geometry import GeometryModel
+from helper.fs import filterModels, listGetFirstValue
+import json
+
+
+def toUrdf(el: GeometryModel):
+    return el.toUrdf()
+
+
+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):
+        dirPath = outPath + Enum.folderPath
+        asm = {}
+        generateSubAssemblyModels = self.generateSubAssembly(assembly)
+        inc = 0
+        for key, value in generateSubAssemblyModels.items():
+            inc += 1
+            if value['assembly'].__len__() != 0:
+                model: Optional[GeometryModel] = listGetFirstValue(
+                    geometryModels, None, lambda x:  x.name == value['assembly'][0])
+
+                if model != None:
+                   
+                    urdfs = list(map(toUrdf, filterModels(
+                        geometryModels,  value['assembly'])))
+                    urdfs.append(listGetFirstValue(
+                        geometryModels, None, lambda x:  x.name == value['part']) .toUrdf())
+                    asm[key] = urdfs
+
+        self.copy(generationFolder=generationFolder,
+                  format='/sdf', outPath=outPath)
+
+        FS.writeFile(data=json.dumps(asm),
+                     fileName=URDF_GENERATOR_FILE, filePath=dirPath)
+        # for el in asm.keys():