Add PDDL, 3D-assets & SDF-URDF generator from Blender Scene Config

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(
+        models.append(listGetFirstValue(filterModels, None, lambda x: x.name == el))
-            filterModels, None, lambda x:  x.name == el))
     return models

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("--generationFolder", help="FreeCad generation folder")
-    parser.add_argument('--outPath', help='save SDF path')
+    parser.add_argument("--outPath", help="save SDF path")
-    parser.add_argument('--world', help='adding sdf world')
+    parser.add_argument("--world", help="adding sdf world")
-    parser.add_argument('--format', help='urdf,sdf,mujoco')
+    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/')
+    geometryFiles = FS.readFilesTypeFolder(args.generationFolder + "/assets/")
-    assemblyStructure = FS.readJSON(
+    assemblyStructure = FS.readJSON(args.generationFolder + "/step-structure.json")
-        args.generationFolder + '/step-structure.json')
 
     geometryModels: list[GeometryModel] = []
     for el in geometryFiles:
-        geometryModels.append(GeometryModel.from_dict(
+        geometryModels.append(
-            FS.readJSON(args.generationFolder + '/assets/' + el)))
+            GeometryModel.from_dict(
-    # if os.path.exists(outPath + 'sdf-generation/'):
+                FS.readJSON(args.generationFolder + "/assets/" + el)
-    #     shutil.rmtree(path=outPath + 'sdf-generation/')
+            )
+        )
+    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,
         )

asp/mocks/sdf/world.sdf

@@ -1,14 +1,7 @@
 <sdf version='1.7'>
   <world name='empty'>
-    <physics name='1ms' type='ignored'>
+   
-      <max_step_size>0.001</max_step_size>
+    
-      <real_time_factor>1</real_time_factor>
-      <real_time_update_rate>1000</real_time_update_rate>
-    </physics>
-    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
-    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
-    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
-    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
     <gravity>0 0 -9.8</gravity>
     <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
     <atmosphere type='adiabatic'/>

asp/src/model/enum.py

@@ -1,2 +1,2 @@
 class Enum:
-    folderPath = 'sdf-generation/';    
+    folderPath = "generation/"

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(
+            result["centerMassX"] = from_union([from_str, from_none], self.centerMassX)
-                [from_str, from_none], self.centerMassX)
         if self.centerMassY is not None:
-            result['centerMassY'] = from_union(
+            result["centerMassY"] = from_union([from_str, from_none], self.centerMassY)
-                [from_str, from_none], self.centerMassY)
         if self.centerMassZ is not None:
-            result['centerMassZ'] = from_union(
+            result["centerMassZ"] = from_union([from_str, from_none], self.centerMassZ)
-                [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__))
+        return (
-                           + '/../../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)
+            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 (
-        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+            FS.readFile(
-                           + '/../../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)
+                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):
+        if pose == False:
-            return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+            return (
-                               + '/../../mocks/sdf/include.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name)
+                FS.readFile(
-        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+                    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)
+                    + "/../../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<model name="assembly">\n'
         sdf += '    <link name="base_link">\n'
         sdf += "        <pose>0 0 0 0 0 0</pose>\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()),
+                sdfJoin += (
-                                   parent=self.name, child=el.name, modelAt=el).toSDF() + '\n'
+                    SdfJoin(
+                        name=str(uuid.uuid4()),
+                        parent=self.name,
+                        child=el.name,
+                        modelAt=el,
+                    ).toSDF()
+                    + "\n"
+                )
 
         sdfJoin += endSDF
-        sdfJoin += '</model>'
+        sdfJoin += "</model>"
         return sdfJoin
 
     def toUrdf(self):
-        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+        return (
-                           + '/../../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)
+            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)
+        )

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('</world') - 1]
-        endWorld = world[world.find('</world') - 1: world.__len__()]
 
+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
+
+
+class GeometryValidateUseCase:
+    def call(geometry) -> str:
+        return

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'
+SDF_FILE_FORMAT = ".sdf"
-CONFIG_PATH = os.path.dirname(os.path.realpath(
+CONFIG_PATH = (
-    __file__)) + '/../../mocks/sdf/model.config'
+    os.path.dirname(os.path.realpath(__file__)) + "/../../mocks/sdf/model.config"
-
+)
- 
 
 
 class SdfSubAssemblyUseCase(Assembly):
-
+    def call(
-    def call(self, geometryModels: list[GeometryModel], assembly: list[str], outPath: str, generationFolder: str, world: bool):
+        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": (
+        self.copy(generationFolder=generationFolder, format="/sdf", outPath=outPath)
-                        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 + '/'
+            path = dirPath + el.name + "/"
             os.makedirs(path)
-            FS.writeFile(data=el.toSDF(), filePath=path,
+            FS.writeFile(
-                         fileName='/model' + SDF_FILE_FORMAT)
+                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=FS.readFile(CONFIG_PATH),
+                filePath=path,
+                fileName="/model" + ".config",
+            )
 
         for key, v in asm.items():
-            FS.writeFile(data=v['assembly'], filePath=dirPath,
+            FS.writeFile(
-                         fileName='/' + key + SDF_FILE_FORMAT)
+                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,
+                FS.writeFile(
-                             fileName='/' + key + SDF_FILE_FORMAT)
+                    data=SdfGenerateWorldUseCase.call(v["assembly"]),
+                    filePath=dirPath,
+                    fileName="/" + key + SDF_FILE_FORMAT,
+                )
 
         FormatterUseCase.call(outPath=outPath, format=SDF_FILE_FORMAT)

asp/src/usecases/urdf_sub_assembly_usecase.py

@@ -6,14 +6,25 @@ import json
 import re
 
 
-URDF_FILE_FORMAT = '.urdf'
+URDF_FILE_FORMAT = ".urdf"
-URDF_GENERATOR_FILE = 'urdf-generation' + '.json'
+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),
+        FS.writeFile(
-                     fileName=URDF_GENERATOR_FILE, filePath=dirPath)
+            data=json.dumps(asm, indent=4),
+            fileName=URDF_GENERATOR_FILE,
+            filePath=dirPath,
+        )

cad_generation/env.json

@@ -1,6 +1,4 @@
 {
-    "doc": "/home/idontsudo/framework/asp/out/disk_and_axis_n.FCStd",
+  "cadFilePath": "/Users/idontsudo/Desktop/asp-example/disk_and_axis_n.FCStd",
-    "out": "/home/idontsudo/framework/asp/out",
+  "outPath": "/Users/idontsudo/Desktop/asp-example/"
-    "resultURL": "http://localhost:3002/assembly/save/out",
-    "projectId": "cubes"
 }

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()

cad_generation/helper/fs.py

@@ -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()

cad_generation/main.py

@@ -1,19 +1,25 @@
-import requests
 import FreeCAD as App
-from helper.fs import FS
+from helper.file_system_repository import FileSystemRepository
-from scenarios.robossembler_freecad_export_scenario import RobossemblerFreeCadExportScenario
+from scenarios.robossembler_freecad_export_scenari import (
-import shutil
+    RobossemblerFreeCadExportScenari,
-import os
+)
+
+
 import FreeCADGui as Gui
 
+#  obj.Support[0][0].Label
+# 'Hex_King'
+
+import FreeCAD as App
+
 
 def main():
-    env = FS.readJSON('./env.json')
+    env = FileSystemRepository.readJSON("./env.json")
-    App.openDocument(env.get('doc'))
+    App.openDocument(env.get("cadFilePath"))
-    RobossemblerFreeCadExportScenario().call(env.get('out'))
+    RobossemblerFreeCadExportScenari.call(env.get("outPath"))
-    # requests.post(url=env.get('resultURL'),  files={'zip': open(env.get('out') + '/' + 'generation.zip', "rb"), 'id':env.get('projectId')})
-    # os.remove('./generation.zip')
     App.closeDocument(App.ActiveDocument.Name)
     freecadQTWindow = Gui.getMainWindow()
     freecadQTWindow.close()
+
+
 main()

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("'", '"')

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",
+            )

cad_generation/scenarios/robossembler_freecad_export_scenario.py

@@ -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',)
-     
-
- 
-
- 

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):
+            if inc != 0:
-                asmStructure[inc] = {
+                asmStructure[inc] = {"child": el, "parents": assembly[0:inc]}
-                    "child": el,
-                    "parents": assembly[0:inc]
-                }
             inc += 1
         objectsFreeCad = App.ActiveDocument.Objects
         asmSolids = {}
         for k, v in asmStructure.items():
-            assemblyParentList = v['parents']
+            assemblyParentList = v["parents"]
-            assemblyChild = v['child']
+            assemblyChild = v["child"]
             for el in assemblyParentList:
                 for solid in objectsFreeCad:
-                    if (el == solid.Label):
+                    if el == solid.Label:
-                        if (asmSolids.get(k) is None):
+                        if asmSolids.get(k) is None:
+                            asmSolids[k] = {
+                                "parents": [],
+                                "child": list(
+                                    filter(
+                                        lambda x: x.Label == assemblyChild,
+                                        objectsFreeCad,
+                                    )
+                                )[0],
+                            }
 
-                            asmSolids[k] = {'parents': [], 'child': list(
+                        asmSolids[k]["parents"].append(solid)
-                                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):
+            if k != 0:
                 App.activeDocument().addObject("Part::Compound", "Compound")
 
-                copyLinks = list(
+                copyLinks = list(map(lambda el: SimpleCopyPartModel(el), v["parents"]))
-                    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.XMax)
-                    geometry['0'].append(boundBox.YMax)
+                    geometry["0"].append(boundBox.YMax)
-                    geometry['0'].append(boundBox.ZMax)
+                    geometry["0"].append(boundBox.ZMax)
 
                     os.makedirs(
-                        path + FolderGenerator.ASSEMBlY.value + '/' + '0000' + str(k))
+                        path + FolderGenerator.ASSEMBlY.value + "/" + "0000" + str(k)
-                    boundBoxChild = v['child'].Shape.BoundBox
+                    )
-                    geometry['1'].append(boundBoxChild.XMax)
+                    boundBoxChild = v["child"].Shape.BoundBox
-                    geometry['1'].append(boundBoxChild.YMax)
+                    geometry["1"].append(boundBoxChild.XMax)
-                    geometry['1'].append(boundBoxChild.ZMax)
+                    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 +
+                    importOBJ.export(
-                                     '/' + '0000' + str(k) + '/' + str(0) + '.obj')
+                        v["child"],
-                    FS.writeFile(json.dumps(geometry), path + FolderGenerator.ASSEMBlY.value +
+                        path
-                                 '/' + '0000' + str(k) + '/', 'translation.json')
+                        + 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
-            
- 

cad_generation/usecases/export_usecase.py

@@ -5,32 +5,53 @@ from model.files_generator import FolderGenerator
 from helper.is_solid import is_object_solid
 from enum import Enum
 
+
 class EXPORT_TYPES(Enum):
-    STL = 'STL'
+    STL = "STL"
-    DAO = 'DAO'
+    DAO = "DAO"
-    OBJ = 'OBJ'
+    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 +
+                        Mesh.export(
-                                    '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.stl')
+                            [el],
-                        meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
+                            path
-                            '/' + el.Label + '.stl'
+                            + "/"
+                            + 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')
+                        importOBJ.export(
-                        meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
+                            [el],
-                            '/' + el.Label + '.obj'
+                            path
-        print(300)    
+                            + "/"
+                            + FolderGenerator.SDF.value
+                            + "/"
+                            + FolderGenerator.MESHES.value
+                            + "/"
+                            + el.Label
+                            + ".obj",
+                        )
+                        meshes[el.Label] = (
+                            "/" + FolderGenerator.MESHES.value + "/" + el.Label + ".obj"
+                        )
         return meshes

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.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(
-    qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - \
+        roll / 2
-        np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
+    ) * np.sin(pitch / 2) * np.sin(yaw / 2)
-    qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + \
+    qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
-        np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
+        roll / 2
-    qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - \
+    ) * np.cos(pitch / 2) * np.sin(yaw / 2)
-        np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
+    qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
-    qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + \
+        roll / 2
-        np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/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':
+    def from_dict(obj: Any) -> "MotionResultModel":
-        assert isinstance(obj, dict)
         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)
+                if isOk:
-                # print()
+                    # sequenceNumber += 1
-                # if(assemblyNumber == 1 and sequenceNumber == 4 or assemblyNumber == 1 and sequenceNumber == 0):
+                    isOk = False
-                if(assemblyNumber == 1 and sequenceNumber == 1 and debug):
-                    debug = False
-                    if(sequenceNumber == 0):
-                            sequenceNumber+=1
-                    print(assemblySequenced)
                     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',
+                self.assemblyDir + "generation/meshes/" + assembly[counter] + ".obj",
-                counter
+                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(
+        motionResult = json.loads(
-            sequenceNumber + 1) + '/' + str(assemblyNumber) + '/' + 'motion_result.json')).read())
+            (
+                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(
+        detailWhichZoneBindings = assemblyParts[assemblyCounter - 2].get("part")
-            'part')
 
-        detailStabilityComputed = assemblyParts[assemblyCounter - 1].get(
+        detailStabilityComputed = assemblyParts[assemblyCounter - 1].get("part")
-            'part')
+        relativeCoordinates = coords.get("relativeCoordinates")
-        relativeCoordinates = coords.get('relativeCoordinates')
 
-        relativeEuler = coords.get('relativeEuler')
+        relativeEuler = coords.get("relativeEuler")
 
         specificEuler = {
-            'yaw': detailWhichZoneBindings.Placement.Rotation.toEuler()[0] - relativeEuler.get('yaw'),
+            "yaw": detailWhichZoneBindings.Placement.Rotation.toEuler()[0]
-            'pitch': detailWhichZoneBindings.Placement.Rotation.toEuler()[1] - relativeEuler.get('pitch'),
+            - relativeEuler.get("yaw"),
-            'roll': detailWhichZoneBindings.Placement.Rotation.toEuler()[2] - relativeEuler.get('roll')
+            "pitch": detailWhichZoneBindings.Placement.Rotation.toEuler()[1]
+            - relativeEuler.get("pitch"),
+            "roll": detailWhichZoneBindings.Placement.Rotation.toEuler()[2]
+            - relativeEuler.get("roll"),
         }
-        quaternion = euler_to_quaternion(specificEuler.get(
+        quaternion = euler_to_quaternion(
-            'yaw'), specificEuler.get('pitch'), specificEuler.get('roll'))
+            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(
+        vector = App.Vector(
-            'x'), relativeCoordinates.get('y'), relativeCoordinates.get('z'))
+            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
+        computedStabilityResult = computedStability(zonePart, detailStabilityComputed)
-        # move_vector = App.Vector(centerVector + vector) - current_center
-        # zonePart.Placement.move(move_vector)
-
-        # computedStabilityResult = computedStability(
-        #     zonePart, detailStabilityComputed)
 
         if sequenceNumber not in self.result.keys():
             self.result[sequenceNumber] = []
 
-        # self.result[sequenceNumber].append({
+        self.result[sequenceNumber].append(
-        #     str(assemblyNumber): assembly,
+            {str(assemblyNumber): assembly, "result": computedStabilityResult}
-        #     "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],
+    return App.Vector(center[0], center[1], center[2])
-                      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_corr = (
-                  clone.Placement.Base).scale(*App.Vector(-0.25, -0.25, -0.25))
+        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().Common.Shapes = [
-    App.ActiveDocument.getObject('Common').ViewObject.ShapeColor = getattr(App.ActiveDocument.getObject(
+        refElement,
-        refElement.Name).getLinkedObject(True).ViewObject, 'ShapeColor', App.ActiveDocument.getObject('Common').ViewObject.ShapeColor)
+        childElement,
-    App.ActiveDocument.getObject('Common').ViewObject.DisplayMode = getattr(App.ActiveDocument.getObject(
+    ]
-        childElement.Name).getLinkedObject(True).ViewObject, 'DisplayMode', App.ActiveDocument.getObject('Common').ViewObject.DisplayMode)
+    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'))
+        zoneDetail = create(el.get("detailForEvaluation"))
-
+        move_second_part_to_match_center(el.get("zoneBindingDetail"), zoneDetail)
-        move_second_part_to_match_center(
+        zoneDetail.Label = "zone_sub_assembly" + str(increment + 1)
-            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(
+        zoneDetailCenterVector = el.get("detailForEvaluation").Shape.CenterOfMass
-            'detailForEvaluation').Shape.CenterOfMass
 
-        el['relativeCoordinates'] = {
+        el["relativeCoordinates"] = {
-            'x': zoneBindingDetailCenterVector.x - zoneDetailCenterVector.x,
+            "x": zoneBindingDetailCenterVector.x - zoneDetailCenterVector.x,
-            'y': zoneBindingDetailCenterVector.y - zoneDetailCenterVector.y,
+            "y": zoneBindingDetailCenterVector.y - zoneDetailCenterVector.y,
-            'z': zoneBindingDetailCenterVector.z - zoneDetailCenterVector.z
+            "z": zoneBindingDetailCenterVector.z - zoneDetailCenterVector.z,
         }
 
-        el['relativeEuler'] = {
+        el["relativeEuler"] = {
-            'yaw': zoneBindingDetail.Placement.Rotation.toEuler()[0] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[0],
+            "yaw": zoneBindingDetail.Placement.Rotation.toEuler()[0]
-            'pitch': zoneBindingDetail.Placement.Rotation.toEuler()[1] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[1],
+            - el.get("detailForEvaluation").Placement.Rotation.toEuler()[0],
-            'roll': zoneBindingDetail.Placement.Rotation.toEuler()[2] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[2]
+            "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')
+        mesh = zoneComputed.get("zonePart")
-        zonesSavePath = meshesPath + mesh.Label + '.obj'
+        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]
+        savePath = zonesSaved[el.get("zonePart").Label]
-        el['zonePart'] = savePath
+        el["zonePart"] = savePath
-        el['detailForEvaluation'] = el['detailForEvaluation'].Label
+        el["detailForEvaluation"] = el["detailForEvaluation"].Label
-        el['zoneBindingDetail'] = el['zoneBindingDetail'].Label
+        el["zoneBindingDetail"] = el["zoneBindingDetail"].Label
 
         json_result = json.dumps(el)
 
-        file_to_open = directoryStableZonesPath + \
+        file_to_open = (
-            'sub_assembly_coords_' + str(counter) + '.json'
+            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')
+    assemblyDir = env.get("aspPath")
-    sequencesJSON = json.loads((open(assemblyDir + 'sequences.json')).read())
+    sequencesJSON = json.loads((open(assemblyDir + "sequences.json")).read())
-    directoryStableZones = assemblyDir + 'stability/zones/'
+    directoryStableZones = assemblyDir + "stability/zones/"
 
-    sequences = sequencesJSON.get('sequences')
+    sequences = sequencesJSON.get("sequences")
-    stepStructure = json.loads(
+    stepStructure = json.loads((open(assemblyDir + "step-structure.json")).read())
-        (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 indexCounter != 0:
-                if (sequencesJoin.get(arrayCounter) == None):
+                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()

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

geometric_feasibility_predicate/README.MD

@@ -1 +1,2 @@
+/Users/idontsudo/Desktop/FreeCAD.app/Contents/MacOS/FreeCAD
 freecadcmd main.py

geometric_feasibility_predicate/env.json

@@ -1,4 +1,5 @@
 {
-    "cadFilePath":"/home/idontsudo/framework/asp/out/disk_and_axis_n (2).FCStd",
+  "cadFilePath": "/Users/idontsudo/Desktop/asp-example/disk_and_axis_n.FCStd",
-    "outPath":"/home/idontsudo/framework/asp/out/"
+  "outPath": "/Users/idontsudo/Desktop/asp-example/",
+  "objectIndentation": 0
 }

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(
+                result[inc][index] = list(
-                    'number') == matrix[index]+1, self.topologyIds))[0].get('name')
+                    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 getAllSolids(self):
+    def isAllObjectsSolids(self) -> List[str]:
-        if (self._solids.__len__() == 0):
+        result = []
         for part in App.ActiveDocument.Objects:
-                if (self.is_object_solid(part)):
+            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:
+            for part in App.ActiveDocument.Objects:
+                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'):
+        if not hasattr(obj.Shape, "Solids"):
-            return False
-        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):
+            if self.matrix.get(el) == None:
-                self.matrixError[el] = 'Not found adjacency ' + el
+                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(
+        result["matrix"] = from_dict(lambda x: from_list(from_str, x), self.matrix)
-            lambda x: from_list(from_str, x), self.matrix)
+        if self.matrixError.values().__len__() == 0:
-        if (self.matrixError.values().__len__() == 0):
+            result["matrixError"] = None
-            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))
+            App.Rotation(App.Vector(0.00, 0.00, 1.00), 0.00),
-        if (detailSquares.get(self.label) is None):
+        )
+        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])
+                            activePart.Shape.distToShape(primitivePart.Shape)[0]
-                        if (collisionResult == 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(
+                                    matrix[model.label].append(renderPrimitive.label)
-                                        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])
+                        part.Shape.distToShape(nextPart.Shape)[0]
-                    if (collisionResult == 0):
+                    )
+                    print(collisionResult)
+                    print("collisionResult")
+                    if collisionResult == 0:
                         matrix[part.Label].append(nextPart.Label)
-        return AdjacencyMatrix(all_parts=GetAllPartsLabelsUseCase(
+
-        ).call(), first_detail=GetFirstDetailUseCase().call(),
+        return AdjacencyMatrix(
-            matrix=matrix
+            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({
+                    detailsToCheck.append({"child": el, "parent": k})
-                        'child': el,
-                        'parent': k
-                    })
     return detailsToCheck
 
-def intersectionComputed(parts):
+
+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.ShapeColor = getattr(
-    App.activeDocument().getObject('Common').ViewObject.DisplayMode = getattr(parts[0].getLinkedObject(True).ViewObject, 'DisplayMode', App.activeDocument().getObject('Common').ViewObject.DisplayMode)
+            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
+        area = App.activeDocument().getObject("Common").Shape.Area
-    App.ActiveDocument.removeObject('Common')
+        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')
+    env = FS.readJSON("env.json")
-    cadFile = env['cadFilePath']
+    cadFilePath = str(env["cadFilePath"])
-    outPath = env['outPath']
+    outPath = str(env["outPath"])
-    if (cadFile == None):
+    objectIndentation = float(env["objectIndentation"])
-        return TypeError('CadFile not found env.json')
+
-    App.open(u'' + cadFile)
+    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 = {
+    intersection_geometry = {"status": True, "recalculations": None}
-        'status':True,
-        'recalculations':None
-    }
     for el in contacts:
-        child = App.ActiveDocument.getObjectsByLabel(el.get('child'))[0]
+        child = App.ActiveDocument.getObjectsByLabel(el.get("child"))[0]
-        parent =  App.ActiveDocument.getObjectsByLabel(el.get('parent'))[0]
+        parent = App.ActiveDocument.getObjectsByLabel(el.get("parent"))[0]
-        area = intersectionComputed([child,parent])
+        area = IntersectionComputedUseCase.call([child, parent])
-        if(area != 0.0):
+        if area != 0.0:
-            if(intersection_geometry.get('recalculations') == None):
+            if intersection_geometry.get("recalculations") == None:
-                intersection_geometry['status'] = False
+                intersection_geometry["status"] = False
-                intersection_geometry['recalculations'] = []
+                intersection_geometry["recalculations"] = []
-            intersection_geometry['recalculations'].append({
+            intersection_geometry["recalculations"].append(
-                'area':area,
+                {"area": area, "connect": el.get("child") + " " + el.get("parent")}
-                '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()
- 

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

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

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()

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,17 +19,31 @@ 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 trimesh
-import os
+
+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:
@@ -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
 
 
-def main():
+# mesh1 = trimesh.load('/Users/idontsudo/framework/asp/out/sdf-generation/meshes/Cube.obj')
-    from argparse import ArgumentParser
+# mesh2 = trimesh.load('/Users/idontsudo/framework/asp/out/sdf-generation/meshes/Cube001.obj')
-    parser = ArgumentParser()
+
-    parser.add_argument('--asp-path', type=str, required=True)
+
-    args = parser.parse_args()
+# # Объединение мешей
-    aspDir = args.asp_dir
+# 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
+
+    # # Коректировка пути до папки с генерацией 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:
+    for el in FS.readFolder(aspDir + 'assemblys'):
+       for e in FS.readFolder(el):
                 try:
                     # Пост обработка .obj обьектов
-            process_mesh(source_dir=el, target_dir=el +
+                    process_mesh(source_dir=e, target_dir=e +
-                         '/process/', subdivide=el, verbose=True)
+                                '/process/', subdivide=e, verbose=True)
-            assemblyDirNormalize.append(el + '/process/')
+                    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,
+                planner_name='rrt',
-                step_size=args.step_size,
+                step_size=None,
-                max_time=args.max_time,
+                max_time=None,
-                seed=args.seed,
+                seed=1,
                 return_path=True,
-                simplify=args.simplify,
+                simplify=False,
-                render=args.render
+                render=False
             )
 
             print(f'Status: {status}, planning time: {t_plan}')

insertion_vector_predicate/requirements.txt

@@ -1,2 +1,3 @@
 spatialmath
 scipy
+uuid

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("--sequencesPath", help="sequences path")
-    parser.add_argument('--outPath', help='save pddl path')
+
+    parser.add_argument("--outPath", help="save pddl path")
+    parser.add_argument("--robossemblerDbPath", help="robossembler_db is require")
     args = parser.parse_args()
 
-    if args.stepStructurePath == None or args.outPath == None:
+    if (
+        args.sequencesPath == None
+        or args.outPath == None
+        or args.robossemblerDbPath == None
+    ):
         parser.print_help()
 
-    data = FS.readJSON(args.stepStructurePath)
+    data = FS.readJSON(args.sequencesPath)
-
+    robossemblerDb = FS.readJSON(args.robossemblerDbPath)
-    assemblyToPddlUseCase = AssemblyToPddlUseCase.call(assembly=data,rootLabel=data[0])
-    FS.writeFile(assemblyToPddlUseCase['problem'] ,args.outPath, 'problem.pddl')
-    FS.writeFile(assemblyToPddlUseCase['domain'] ,args.outPath, 'domain.pddl')
 
+    assemblyToPddlUseCase = AssemblyToPddlUseCase.call(
+        assembly=data, robossemblerDb=robossemblerDb
+    )
 
+    FS.writeFile(assemblyToPddlUseCase["problem"], args.outPath, "problem.pddl")
+    FS.writeFile(assemblyToPddlUseCase["domain"], args.outPath, "domain.pddl")

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))
+        )
+    )
+)

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)
+        )
+    )
+)-=

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),
+            )
+        )

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):
+    def call(assembly: List[str], robossemblerDb: RobossemblerAssets):
-        partType = UserType("part")
+        result = ""
-        assemblyType = UserType('assembly')
+        robots = robossemblerDb.getAllRobotsName()
-
+        for robot in robots:
-        objectsPartPddl = []
+            result += (
-        objectsAsmToPddl = []
+                FS.readFile(
-        i = 0
+                    os.path.dirname(os.path.realpath(__file__))
-        for el in assembly:
+                    + "/../../mocks"
-            objectsPartPddl.append(Object(el, partType))
+                    + "/problem.txt"
-
+                )
-        problem = Problem(rootLabel)
+                .replace(
-
+                    "${types}",
-        for el in objectsPartPddl:
+                    typeGenerator(assembly, "part")
-            problem.add_object(el)
+                    + "\n        "
-        i = 0
+                    + typeGenerator(
-        for el in objectsPartPddl:
+                        list(
-            problem.add_object(Object('subasm' + str(i), assemblyType))
+                            map(
-            objectsAsmToPddl.append(Object('subasm' + str(i), assemblyType))
+                                lambda x: "subasm" + str(x),
-            i = i+1
+                                list(range(assembly.__len__())),
-
+                            )
-        connected = Fluent('part-of', BoolType(),
+                        ),
-                           l_from=partType, l_to=assemblyType)
+                        "assembly",
-        assemblyOrder = Fluent('assembly_order', BoolType(),
+                        robot,
-                               l_from=assemblyType, l_to=assemblyType)
+                    ),
-        i = 0
+                )
-        for el in objectsPartPddl:
+                .replace(
-            problem.set_initial_value(connected(el, objectsAsmToPddl[i]), True)
+                    "${assembled}",
-            i = i+1
+                    assemblyTagGenerate(assembly),
-        goal = Fluent(rootLabel)
+                )
-
+                .replace(
-        problem.add_goal(connected(objectsPartPddl[objectsPartPddl.__len__(
+                    "${part_at}",
-        ) - 1], objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]),)
+                    doubleTagGenerate(
-        
+                        instances=assembly, predicates="part_at", tag="workspace1"
-        i = 0
+                    ),
-        for el in objectsAsmToPddl:
+                )
-            if objectsAsmToPddl[i-1] != objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]:
+            ).replace("${target}", "subasm" + str(assembly.__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]),)
         return {
-            "problem": unified_planning.io.PDDLWriter(problem).get_problem(),
+            "problem": result,
-            'domain':  FS.readFile(os.path.dirname(os.path.realpath(__file__)) + '/../../mocks' + '/domain.txt'),
+            "domain": FS.readFile(
+                os.path.dirname(os.path.realpath(__file__))
+                + "/../../mocks"
+                + "/domain.txt"
+            ),
         }

pddl/unit.test.py

@@ -6,21 +6,28 @@ 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 определяет зоны
+#

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()

robossembler_scene_builder/mocks/light_sdf.xml

@@ -0,0 +1,12 @@
+<light type="{type_light}" name="{name_light}">
+    <pose>{x} {y} {z} {roll} {pitch} {yaw}</pose>
+    <diffuse>{r} {g} {b} {a}</diffuse>
+    <specular>.1 .1 .1 1</specular>
+    <attenuation>
+        <range>20</range>
+        <linear>0.2</linear>
+        <constant>0.8</constant>
+        <quadratic>0.01</quadratic>
+    </attenuation>
+    <cast_shadows>false</cast_shadows>
+</light>

robossembler_scene_builder/mocks/model_include_sdf.xml

@@ -0,0 +1,7 @@
+<model name="{name}">
+  <pose>{x} {y} {z} {roll} {pitch} {yaw}</pose>
+  <include>
+    <name>{name}</name>
+    <uri>model://{uri}</uri>
+  </include>
+</model>

robossembler_scene_builder/mocks/world.xml

@@ -0,0 +1,105 @@
+<?xml version="1.0"?>
+<sdf version='1.9'>
+  <world name='mir'>
+    <physics name='1ms' type='{engine_type}'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <plugin
+      name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system' />
+    <plugin
+      name='ignition::gazebo::systems::UserCommands'
+      filename='ignition-gazebo-user-commands-system' />
+    <plugin
+      name='ignition::gazebo::systems::SceneBroadcaster'
+      filename='ignition-gazebo-scene-broadcaster-system' />
+    <plugin
+      name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system' />
+    <plugin
+      name="ignition::gazebo::systems::Sensors" filename="ignition-gazebo-sensors-system">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <gravity>{gravity_x} {gravity_y} {gravity_z}</gravity>
+
+    <magnetic_field>6e-06
+    2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic' />
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>false</shadows>
+    </scene>
+    <gui fullscreen="0">
+      <plugin filename="GzScene3D" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>1.0 1.0 1.0</ambient_light>
+        <background_color>0.4 0.6 1.0</background_color>
+        <camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
+      </plugin>
+      <plugin filename="WorldStats" name="World stats">
+        <ignition-gui>
+          <title>World stats</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">110</property>
+          <property type="double" key="width">290</property>
+          <property type="double" key="z">1</property>
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="right" target="right" />
+            <line own="bottom" target="bottom" />
+          </anchors>
+        </ignition-gui>
+        <sim_time>true</sim_time>
+        <real_time>true</real_time>
+        <real_time_factor>true</real_time_factor>
+        <iterations>true</iterations>
+      </plugin>
+    </gui>
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <model name='ground'>
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+    {models} </world>
+</sdf>

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),
+            )
+        )

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

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()
 
+
+main()

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/' + \
+            file_to_open = outPath + "/generation/" + str(assemblyCount) + ".urdf"
-                str(assemblyCount) + '.urdf'
+            f = open(file_to_open, "w", encoding="utf-8", errors="ignore")
-            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,
+        urdfs = self.urdfLoader(
-                                urdfGeneration=urdfGeneration, outPath=outPath)
+            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(
+                    resultCoords.append(
-                        x=rot[0], y=rot[1], z=rot[2]), position=Coords(x=pos[0], y=pos[1], z=pos[2])))
+                        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' + \
+                pathSaveResultAssemblyFolder = (
-                    '/' + str(activeAssemblyNumber + 1) + '/'
+                    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]
+                    subAssembly = assemblesStructures[activeAssemblyNumber][
+                        0 : subAssemblyNumber + 1
+                    ]
+                    print(subAssembly)
+
+                    if subAssembly == [
+                        "disk_top",
+                        "disk_middel",
+                    ]:
                         asm = []
                         for el in subAssembly:
                             asm.append(el)
 
                         resultSimulationStates = self.executeSimulation(
-                        assembly=asm, outPath=aspPath, urdfGeneration=urdfGeneration, duration=1000)
+                            assembly=asm,
+                            outPath=aspPath,
+                            urdfGeneration=urdfGeneration,
+                            duration=1000,
+                        )
 
-                    pathSaveResultSubAssemblyFolder = aspPath + 'stability' + '/' + \
+                        pathSaveResultSubAssemblyFolder = (
-                        str(activeAssemblyNumber + 1) + \
+                            aspPath
-                        '/' + str(subAssemblyNumber) + '/'
+                            + "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 + '/' +
+                        f = open(
-                             'motion_result.json', 'w', encoding='utf-8', errors='ignore')
+                            pathSaveResultSubAssemblyFolder
-                    f.write(json.dumps(results,
+                            + "/"
-                                       ensure_ascii=False, indent=4))
+                            + "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: ' +
+                        print("process complete: " + str(percentageOfCompletion) + "%")
-                          str(percentageOfCompletion) + '%')
         except Exception as e:
             print(e)