Экспорт подсборок с мешами (SDF) и плана сборки (PDDL) из FreeCAD в виде архива zip

.gitignore

@@ -104,6 +104,7 @@ ENV/
 *.blend1
 install_plugin_cad.sh
 .vscode
+.DS_Store
 
 # emacs backup files
 ~*

cg/freecad/Frames/Frames.py

@@ -13,7 +13,7 @@
 # License along with this library. If not, see <http://www.gnu.org/licenses/>.
 import FreeCAD
 import Tools
-from usecases.asm4parser_usecase import Asm4StructureParseUseCase
+from scenarios.robossembler_freecad_export_scenario import RobossemblerFreeCadExportScenario
 
 if FreeCAD.GuiUp:
     import FreeCADGui
@@ -311,8 +311,9 @@ Tools.spawnClassCommand("FrameCommand",
                           {"Pixmap": str(os.path.join(icondir, "frame.svg")),
                            "MenuText": "Make a free frame",
                            "ToolTip": "Make a freestanding reference frame."})
+
 Tools.spawnClassCommand("ASM4StructureParsing",
-                          Asm4StructureParseUseCase().initParse,
+                          RobossemblerFreeCadExportScenario().call,
                           {"Pixmap": str(os.path.join(icondir, "assembly4.svg")),
                            "MenuText": "Make a ASM4 parsing",
                            "ToolTip": "Make a ASM4 1"})

cg/freecad/Frames/helper/fs.py

@@ -0,0 +1,14 @@
+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', )
+
+        f.write(data)

cg/freecad/Frames/helper/is_solid.py

@@ -0,0 +1,18 @@
+import FreeCAD
+
+
+def is_object_solid(obj):
+    """If obj is solid return True"""
+    if not isinstance(obj, FreeCAD.DocumentObject):
+        return False
+
+    if not hasattr(obj, 'Shape'):
+        return False
+
+    if not hasattr(obj.Shape, 'Solids'):
+        return False
+
+    if len(obj.Shape.Solids) == 0:
+        return False
+
+    return True

cg/freecad/Frames/model/files_generator.py

@@ -0,0 +1,12 @@
+from enum import Enum
+
+
+class FilesGenerator(Enum):
+    DETAIL = 'detail.json'
+    ASSEMBLY = 'assembly.json'
+
+
+class FolderGenerator(Enum):
+    MESHES = 'meshes'
+    ASSETS = 'assets'
+    SDF = 'sdf'

cg/freecad/Frames/model/geometry_part.py

@@ -0,0 +1,86 @@
+from typing import Any, TypeVar, Type, cast
+
+
+T = TypeVar("T")
+
+
+def from_float(x: Any) -> float:
+    assert isinstance(x, (float, int)) and not isinstance(x, bool)
+    return float(x)
+
+
+def to_float(x: Any) -> float:
+    assert isinstance(x, float)
+    return x
+
+
+def to_class(c: Type[T], x: Any) -> dict:
+    assert isinstance(x, c)
+    return cast(Any, x).to_dict()
+
+
+class Axis:
+    x: float
+    y: float
+    z: float
+
+    def __init__(self, x: float, y: float, z: float) -> None:
+        self.x = x
+        self.y = y
+        self.z = z
+
+    @staticmethod
+    def from_dict(obj: Any) -> 'Axis':
+        assert isinstance(obj, dict)
+        x = from_float(obj.get("x"))
+        y = from_float(obj.get("y"))
+        z = from_float(obj.get("z"))
+        return Axis(x, y, z)
+
+    def to_dict(self) -> dict:
+        result: dict = {}
+        result["x"] = to_float(self.x)
+        result["y"] = to_float(self.y)
+        result["z"] = to_float(self.z)
+        return result
+
+
+class GeometryPart:
+    euler: Axis
+    position: Axis
+    rotation: Axis
+    center: Axis
+
+    def __init__(self, euler: Axis, position: Axis, rotation: Axis, center: Axis) -> None:
+        self.euler = euler
+        self.position = position
+        self.rotation = rotation
+        self.center = center
+
+    @staticmethod
+    def from_dict(obj: Any) -> 'GeometryPart':
+        assert isinstance(obj, dict)
+        euler = Axis.from_dict(obj.get("euler"))
+        position = Axis.from_dict(obj.get("position"))
+        rotation = Axis.from_dict(obj.get("rotation"))
+        center = Axis.from_dict(obj.get("center"))
+        return GeometryPart(euler, position, rotation, center)
+
+    def to_dict(self) -> dict:
+        result: dict = {}
+        result["euler"] = to_class(Axis, self.euler)
+        result["position"] = to_class(Axis, self.position)
+        result["rotation"] = to_class(Axis, self.rotation)
+        result["center"] = to_class(Axis, self.center)
+        return result
+
+    def toJson(self) -> str:
+        return str(self.to_dict()).replace('\'', '"')
+
+
+def geometry_part_from_dict(s: Any) -> GeometryPart:
+    return GeometryPart.from_dict(s)
+
+
+def geometry_part_to_dict(x: GeometryPart) -> Any:
+    return to_class(GeometryPart, x)

cg/freecad/Frames/model/sdf_geometry_model.py

@@ -0,0 +1,107 @@
+import json
+
+
+def from_str(x):
+    assert isinstance(x, str)
+    return x
+
+
+def from_none(x):
+    assert x is None
+    return x
+
+
+def from_union(fs, x):
+    for f in fs:
+        try:
+            return f(x)
+        except:
+            pass
+    assert False
+
+
+def to_class(c, x):
+    assert isinstance(x, c)
+    return x.to_dict()
+
+
+class SdfGeometryModel:
+    def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction):
+        self.name = name
+        self.ixx = ixx
+        self.ixy = ixy
+        self.ixz = ixz
+        self.iyy = iyy
+        self.izz = izz
+        self.massSDF = massSDF
+        self.posX = posX
+        self.posY = posY
+        self.posZ = posZ
+        self.eulerX = eulerX
+        self.eulerY = eulerY
+        self.eulerZ = eulerZ
+        self.iyz = iyz
+        self.stl = stl
+        self.friction = friction
+
+    @staticmethod
+    def from_dict(obj):
+        assert isinstance(obj, dict)
+        name = from_union([from_str, from_none], obj.get("name"))
+        ixx = from_union([from_str, from_none], obj.get("ixx"))
+        ixy = from_union([from_str, from_none], obj.get("ixy"))
+        ixz = from_union([from_str, from_none], obj.get("ixz"))
+        iyy = from_union([from_str, from_none], obj.get("iyy"))
+        izz = from_union([from_str, from_none], obj.get("izz"))
+        massSDF = from_union([from_str, from_none], obj.get("massSDF"))
+        posX = from_union([from_str, from_none], obj.get("posX"))
+        posY = from_union([from_str, from_none], obj.get("posY"))
+        posZ = from_union([from_str, from_none], obj.get("posZ"))
+        eulerX = from_union([from_str, from_none], obj.get("eulerX"))
+        eulerY = from_union([from_str, from_none], obj.get("eulerY"))
+        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") )
+        friction = from_union([from_str, from_none], obj.get("friction"))
+        return SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz,stl,friction)
+
+    def to_dict(self):
+        result = {}
+        if self.name is not None:
+            result["name"] = from_union([from_str, from_none], self.name)
+        if self.ixx is not None:
+            result["ixx"] = from_union([from_str, from_none], self.ixx)
+        if self.ixy is not None:
+            result["ixy"] = from_union([from_str, from_none], self.ixy)
+        if self.ixz is not None:
+            result["ixz"] = from_union([from_str, from_none], self.ixz)
+        if self.iyy is not None:
+            result["iyy"] = from_union([from_str, from_none], self.iyy)
+        if self.izz is not None:
+            result["izz"] = from_union([from_str, from_none], self.izz)
+        if self.massSDF is not None:
+            result["massSDF"] = from_union([from_str, from_none], self.massSDF)
+        if self.posX is not None:
+            result["posX"] = from_union([from_str, from_none], self.posX)
+        if self.posY is not None:
+            result["posY"] = from_union([from_str, from_none], self.posY)
+        if self.posZ is not None:
+            result["posZ"] = from_union([from_str, from_none], self.posZ)
+        if self.eulerX is not None:
+            result["eulerX"] = from_union([from_str, from_none], self.eulerX)
+        if self.eulerY is not None:
+            result["eulerY"] = from_union([from_str, from_none], self.eulerY)
+        if self.eulerZ is not None:
+            result["eulerZ"] = from_union([from_str, from_none], self.eulerZ)
+        if self.iyz is not None:
+            result["iyz"] = from_union([from_str, from_none], self.iyz)
+        if self.stl is not None:
+            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)
+        return result
+
+    def toJSON(self) -> str:
+        return str(self.to_dict()).replace('\'', '"')
+
+ 

cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py

@@ -0,0 +1,70 @@
+import FreeCAD
+from usecases.export_usecase import 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
+from PySide import QtGui
+import os
+import ImportGui
+import shutil
+
+
+class RobossemblerFreeCadExportScenario:
+
+    def call(self):
+
+        path = self.qtGuiFeature()
+        if path == None:
+            return
+        self.systemHelper(path)
+
+    def qtGuiFeature(self):
+        if FreeCAD.ActiveDocument == None:
+            FreeCAD.Console.PrintError("No active document")
+
+        p = QtGui.QFileDialog.getExistingDirectoryUrl()
+        path = p.path()
+
+        if path == '':
+            return None
+        return path
+
+    def systemHelper(self, path: str):
+
+        root_label = FreeCAD.ActiveDocument.RootObjects[0].Label
+        directory = path + '/' + root_label
+        
+        if not os.path.exists(directory):
+            os.makedirs(directory)
+            
+        __objs__ = FreeCAD.ActiveDocument.RootObjects
+
+        os.makedirs(directory + '/' + FolderGenerator.ASSETS.value)
+        os.makedirs(directory + '/' + FolderGenerator.SDF.value)
+        os.makedirs(directory + '/' + FolderGenerator.SDF.value + '/' + FolderGenerator.MESHES.value)
+        f = open(directory + "/step-structure.json", "w")
+        f.write(AssemblyParseUseCase().toJson())
+        f.close()
+        self.geometry(directory)
+         
+        ImportGui.export(__objs__, directory + '/' + 'assembly.step')
+       
+        shutil.make_archive(directory, 'zip', directory)
+
+        shutil.rmtree(directory)
+        return True
+
+    def geometry(self, outPutsPath: str):
+        meshesExportUseCase = ExportUseCase.call(outPutsPath)
+        for el in SdfGeometryUseCase.call(meshesExportUseCase):
+            FS.writeFile(el.toJSON(),   outPutsPath +
+                         '/' + FolderGenerator.ASSETS.value + '/', el.name + '.json',)
+             
+     
+
+ 
+
+ 

cg/freecad/Frames/usecases/asm4parser_usecase.py

@@ -1,53 +0,0 @@
-import FreeCAD as App
-
-class Asm4StructureParseUseCase:
-    _parts = []
-    _label = []
-
-    def getSubPartsLabel(self, group):
-        groupLabel = []
-        for el in group:
-            if str(el) == '<Part::PartFeature>':
-                groupLabel.append(el.Label)
-        return groupLabel
-
-    def parseLabel(self, nextGroup, label, level=2, nextGroupParse=0):
-        if nextGroup.__len__() == nextGroupParse:
-            return
-        else:
-            groupParts = []
-
-            for el in nextGroup:
-                if str(el) == '<App::Link object>':
-                    groupParts.append(el)
-
-            for el in groupParts:
-                if str(el) == '<App::Link object>':
-                    label.append({
-                        "level": level,
-                        "attachedTo": el.AttachedTo.split('#'),
-                        "label": el.Label,
-                        "axis": self.getSubPartsLabel(el.Group)
-                    })
-
-    def initParse(self):
-
-        model = App.ActiveDocument.RootObjects[1]
-        self._label.append({
-            "level": 1,
-            "attachedTo": "Parent Assembly",
-            "label": model.Label,
-            "axis": self.getSubPartsLabel(model.Group)
-        })
-        for parent in model.Group:
-            if str(parent) == '<App::Link object>':
-                self._label.append({
-                    "level": 1,
-                    "attachedTo": parent.AttachedTo.split('#'),
-                    "label": parent.Label,
-                    "axis": self.getSubPartsLabel(parent.Group)
-                })
-        print(self._label)
-
-
- 

cg/freecad/Frames/usecases/assembly_parse_usecase.py

@@ -0,0 +1,42 @@
+import FreeCAD as App
+from helper.is_solid import is_object_solid
+ 
+
+class AssemblyParseUseCase:
+    _parts = []
+    _asm = []
+
+    def __init__(self) -> None:
+        self.initParse()
+        pass
+
+    
+
+    def initParse(self):
+        for el in  App.ActiveDocument.Objects:
+            if(is_object_solid(el)):
+                self._asm.append(el.Label)
+
+    def toJson(self):
+        return str(self._asm).replace('\'', "\"")
+
+    def getSubPartsLink(self, group):
+        groupLink = {}
+        for el in group:
+            if (is_object_solid(el)):
+                if str(el.Shape).find('Solid') != -1:
+                    if groupLink.get(el.Label) == None:
+                        groupLink[el.Label] = []
+                    for i in el.Group:
+
+                        if str(i).find('Pad') != -1:
+                            groupLink[el.Label].append(i)
+        if groupLink.__len__() == 0:
+            return None                    
+        return groupLink
+
+    def getLinkedProperty(self):
+        return self._asm
+
+  
+

cg/freecad/Frames/usecases/export_usecase.py

@@ -0,0 +1,16 @@
+import importDAE
+import FreeCAD as App
+from model.files_generator import FolderGenerator
+from helper.is_solid import is_object_solid
+
+
+class ExportUseCase:
+    def call(path):
+        meshes = {}
+        for el in App.ActiveDocument.Objects:
+            if (is_object_solid(el)):
+                importDAE.export([el], path + '/' + FolderGenerator.SDF.value +
+                            '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae')
+                meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
+                    '/' + el.Label + '.dae'
+        return meshes

cg/freecad/Frames/usecases/geometry_usecase.py

@@ -0,0 +1,54 @@
+
+import FreeCAD as App
+from helper.is_solid import is_object_solid
+
+
+class GeometryUseCase:
+    def call() -> dict:
+        labels = []
+        for el in App.ActiveDocument.Objects:
+             
+            if is_object_solid(el):
+                labels.append(el.Label)
+                
+                geometry = {
+                    "euler": {
+                        "x": None,
+                        "y": None,
+                        "z": None
+                    },
+                    "position": {
+                        "x": None,
+                        "y": None,
+                        "z": None
+                    },
+                    "rotation": {
+                        "x": None,
+                        "y": None,
+                        "z": None
+                    },
+                    "center": {
+                        "x": None,
+                        "y": None,
+                        "z": None
+                    },
+                     
+                }
+
+                boundBox = el.Shape.BoundBox
+                geometry["center"]["x"] = boundBox.Center.x
+                geometry["center"]["y"] = boundBox.Center.y
+                geometry["center"]["z"] = boundBox.Center.z
+                geometry["position"]['x'] = boundBox.XMax
+                geometry["position"]['y'] = boundBox.YMax
+                geometry["position"]['z'] = boundBox.ZMax
+                rotation = el.Placement.Rotation
+                geometry["rotation"]['x'] = rotation.Axis.z
+                geometry["rotation"]['y'] = rotation.Axis.y
+                geometry["rotation"]['z'] = rotation.Axis.z
+                euler = el.Placement.Rotation.toEuler()
+                geometry["euler"]['x'] = euler[0]
+                geometry["euler"]['y'] = euler[1]
+                geometry["euler"]['z'] = euler[2]
+
+        return {"geometry": geometry, "labels": labels, "label": el.Label}

cg/freecad/Frames/usecases/get_sdf_geometry_usecase.py

@@ -0,0 +1,63 @@
+import FreeCAD as App
+from model.sdf_geometry_model import SdfGeometryModel
+ 
+from helper.is_solid import is_object_solid
+
+
+class SdfGeometryUseCase:
+    def call(stlPaths:dict) -> list[SdfGeometryModel]:
+        materialSolid = {}
+        for el in App.ActiveDocument.Objects:
+            if str(el) == '<App::MaterialObjectPython object>':   
+                friction = el.Material.get('SlidingFriction')
+                for i in el.References:
+                    materialSolid[i[0].Label] = friction
+        geometry = []
+        for el in App.ActiveDocument.Objects:
+            if is_object_solid(el):
+                com = el.Shape.CenterOfMass
+                mass = el.Shape.Mass
+                inertia = el.Shape.MatrixOfInertia
+                pos = el.Shape.Placement
+                inertia = el.Shape.MatrixOfInertia
+                name = el.Label
+                ixx = str(inertia.A11 / 1000000)
+                ixy = str(inertia.A12 / 1000000)
+                ixz = str(inertia.A13 / 1000000)
+                iyy = str(inertia.A22 / 1000000)
+                iyz = str(inertia.A23 / 1000000)
+                izz = str(inertia.A33 / 1000000)
+                massSDF = str(mass / 1000000)
+                posX = str(pos.Base[0] / 1000000)
+                posY = str(pos.Base[1] / 1000000)
+                posZ = str(pos.Base[2] / 1000000)
+                eulerX = str(pos.Rotation.toEuler()[0])
+                eulerY = str(pos.Rotation.toEuler()[1])
+                eulerZ = str(pos.Rotation.toEuler()[2])
+                
+                geometry.append(
+                    SdfGeometryModel(
+                        stl=stlPaths.get(el.Label),
+                        name=name, 
+                        ixx=ixx,
+                        ixz=ixz, 
+                        ixy=ixy, 
+                        iyy=iyy,
+                        iyz=iyz,
+                        izz=izz,
+                        massSDF=massSDF,
+                        posX=posX,
+                        posY=posY,
+                        posZ=posZ,
+                        eulerX=eulerX, 
+                        eulerY=eulerY, 
+                        eulerZ=eulerZ,
+                        friction=materialSolid.get(el.Label) or '',
+                    )
+                )
+        return geometry
+
+ 
+ 
+
+

pddl/helper/fs.py

@@ -12,6 +12,8 @@ class FS:
         f = open(file_to_open, 'w', )
 
         f.write(data)
+    def readFile(path):
+        return open(path).read()
 
     def readFile(path:str):
         return open(path).read()

pddl/main.py

@@ -2,25 +2,23 @@ import argparse
 from helper.fs import FS
 from src.model.asm4_structure import Asm4Structure
 
-from src.usecases.asm4_to_assembly_use_case import Asm4ToAssemblyUseCase
 from src.usecases.assembly_to_pddl_use_case import AssemblyToPddlUseCase
  
  
-
+# python3 main.py --stepStructurePath /Users/idontsudo/robo/Cube3/step-structure.json --outPath /Users/idontsudo/robo/Cube3/pddl/
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
-    parser.add_argument('--asm4Path', help='asm4 json FreeCad')
+    parser.add_argument('--stepStructurePath', help='json step by FreeCad')
     parser.add_argument('--outPath', help='save pddl path')
     args = parser.parse_args()
     
-    if args.asm4Path == None or args.outPath == None:
+    if args.stepStructurePath == None or args.outPath == None:
         parser.print_help()
     
-    data = FS.readJSON(args.asm4Path)
+    data = FS.readJSON(args.stepStructurePath)
-    asm4 = Asm4Structure.parse(data)
+
-    asm4usecase = Asm4ToAssemblyUseCase().call(asm4)
+    assemblyToPddlUseCase = AssemblyToPddlUseCase.call(assembly=data,rootLabel=data[0])
-    assemblyToPddlUseCase = AssemblyToPddlUseCase.call(assembly=asm4usecase['asm'],rootLabel=asm4usecase['rootLabel'])
     FS.writeFile(assemblyToPddlUseCase['problem'] ,args.outPath, 'problem.pddl')
     FS.writeFile(assemblyToPddlUseCase['domain'] ,args.outPath, 'domain.pddl')
     

pddl/mocks/asm4-mock.json

@@ -1,86 +0,0 @@
-[
-    {
-        "level": 1,
-        "attachedTo": "Parent Assembly",
-        "label": "Model",
-        "axis": [
-            "LCS_Origin"
-        ]
-    },
-    {
-        "level": 1,
-        "attachedTo": [
-            "Parent Assembly",
-            "LCS_Origin"
-        ],
-        "label": "CubePart001",
-        "axis": [
-            "LCS_0"
-        ]
-    },
-    {
-        "level": 1,
-        "attachedTo": [
-            "CubePart001",
-            "LCS_0"
-        ],
-        "label": "CubePart002",
-        "axis": [
-            "LCS_0"
-        ]
-    },
-    {
-        "level": 1,
-        "attachedTo": [
-            "CubePart001",
-            "LCS_0"
-        ],
-        "label": "CubePart003",
-        "axis": [
-            "LCS_0"
-        ]
-    },
-    {
-        "level": 1,
-        "attachedTo": [
-            ""
-        ],
-        "label": "CubePart004",
-        "axis": [
-            "LCS_0"
-        ]
-    },
-    {
-        "level": 1,
-        "attachedTo": [
-            "CubePart001",
-            "LCS_0"
-        ],
-        "label": "CubePart005",
-        "axis": [
-            "LCS_0"
-        ]
-    },
-    {
-        "level": 1,
-        "attachedTo": [
-            "CubePart005",
-            "LCS_0"
-        ],
-        "label": "CubePart006",
-        "axis": [
-            "LCS_0"
-        ]
-    },
-    {
-        "level": 1,
-        "attachedTo": [
-            "CubePart006",
-            "LCS_0"
-        ],
-        "label": "CubePart007",
-        "axis": [
-            "LCS_0"
-        ]
-    }
-]

pddl/mocks/step-mock.json

@@ -0,0 +1 @@
+["Cube3", "Cube1", "Cube2"]

pddl/src/usecases/asm4_to_assembly_use_case.py

@@ -1,19 +0,0 @@
-from src.model.asm4_structure import Asm4Structure 
-from typing import List 
- 
-
-
-class Asm4ToAssemblyUseCase:
-    _asm4Assembly = []
-    def call(self,asm4:List[Asm4Structure] ):
-        rootPart = asm4[0]
-        self._asm4Assembly.append(rootPart.label)
-        for el in asm4[1:asm4.__len__()]:
-            self.parse(lead=el)
-        return {'asm':self._asm4Assembly[1:self._asm4Assembly.__len__()],'rootLabel':rootPart.label}
-    def parse(self,lead:Asm4Structure ):
-        if lead.attached_to[0] == 'Parent Assembly':
-            self._asm4Assembly.append(lead.label)
-        for i in self._asm4Assembly:
-            if i == lead.attached_to[0]:
-                self._asm4Assembly.append(lead.label)

pddl/src/usecases/assembly_to_pddl_use_case.py

@@ -7,6 +7,7 @@ import os
 
 class AssemblyToPddlUseCase:
     def call(assembly: List[str], rootLabel: str):
+        print(assembly)
         partType = UserType("part")
         assemblyType = UserType('assembly')
 
@@ -36,8 +37,8 @@ class AssemblyToPddlUseCase:
         goal = Fluent(rootLabel)
         problem.add_goal(connected(objectsPartPddl[objectsPartPddl.__len__(
         ) - 1], objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]),)
-
         return {
             "problem": unified_planning.io.PDDLWriter(problem).get_problem(),
             'domain':  FS.readFile(os.path.dirname(os.path.realpath(__file__)) + '/../../mocks' + '/domain.txt'),
         }
+ 

pddl/unit.test.py

@@ -1,5 +1,4 @@
 import unittest
-from src.usecases.asm4_to_assembly_use_case import Asm4ToAssemblyUseCase
 from src.usecases.assembly_to_pddl_use_case import AssemblyToPddlUseCase
 from src.model.asm4_structure import Asm4Structure
  
@@ -7,17 +6,16 @@ from helper.fs import FS
 import os
 
 
-mock = FS.readJSON(os.path.dirname(os.path.realpath(__file__)) + '/mocks/asm4-mock.json')
+mock = FS.readJSON(os.path.dirname(os.path.realpath(__file__)) + '/mocks/step-mock.json')
 
-asm4 = Asm4Structure.parse(mock)
+  
- 
 
-asm4usecase = Asm4ToAssemblyUseCase().call(asm4)
+assemblyToPddl =  AssemblyToPddlUseCase.call(assembly=mock,rootLabel=mock[0])
-assemblyToPddl =  AssemblyToPddlUseCase.call(assembly=asm4usecase['asm'],rootLabel=asm4usecase['rootLabel'])
 
  
 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)

sdf/helper/fs.py

@@ -0,0 +1,23 @@
+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', )
+
+        f.write(data)
+    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
+

sdf/main.py

@@ -0,0 +1,71 @@
+import argparse
+import shutil
+from distutils.dir_util import copy_tree
+import asyncio
+from helper.fs import FS
+from src.usecases.generate_world import SdfGenerateWorldUseCase
+from src.model.sdf_geometry import SdfGeometryModel
+from src.usecases.sdf_sub_assembly_usecase import SdfSubAssemblyUseCase
+
+import os
+import typing
+import xmlformatter
+
+ 
+
+# python3 main.py  --generationFolder /Users/idontsudo/robo/Cube3/ --outPath /Users/idontsudo/robo/ --world true
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--generationFolder', help='FreeCad generation folder')
+    parser.add_argument('--outPath', help='save SDF path')
+    parser.add_argument('--world', help='adding sdf world')
+
+    args = parser.parse_args()
+
+    if args.generationFolder == None or args.outPath == None:
+        parser.print_help()
+    outPath = args.outPath
+    
+    geometryFiles = FS.readFilesTypeFolder(args.generationFolder + '/assets/')
+    assemblyStructure = FS.readJSON(
+        args.generationFolder + '/step-structure.json')
+
+    sdfGeometryModels: list[SdfGeometryModel] = []
+    for el in geometryFiles:
+        sdfGeometryModels.append(SdfGeometryModel.from_dict(
+            FS.readJSON(args.generationFolder + '/assets/' + el)))
+    sdfSubAssemblyUseCase = SdfSubAssemblyUseCase().call(
+        sdfGeometryModels, assemblyStructure,)
+    
+    if os.path.exists(outPath + 'sdf-generation/'):
+        shutil.rmtree(path=outPath + 'sdf-generation/')
+    
+    copy_tree(args.generationFolder + 'sdf/', outPath + 'sdf-generation/')
+    dirPath = outPath + 'sdf-generation/'
+    for el in sdfGeometryModels:
+        path = dirPath + el.name + '/'
+        os.makedirs(path)
+        FS.writeFile(data=el.toSDF(), filePath=path,
+                     fileName='/model' + '.sdf')
+        FS.writeFile(data=FS.readFile(os.path.dirname(os.path.realpath(__file__))
+                                      + '/mocks/sdf/model.config'), filePath=path, fileName='/model' + '.config')
+    
+     
+    if(args.world == None):
+        for key, v in sdfSubAssemblyUseCase.items():
+            FS.writeFile(data=v['assembly'], filePath=dirPath,
+                     fileName='/' + key + '.sdf')
+    
+    else:
+          for key, v in sdfSubAssemblyUseCase.items():
+                FS.writeFile(data=SdfGenerateWorldUseCase.call(v['assembly']), filePath=dirPath,
+                     fileName='/' + key + '.sdf')
+    formatter = xmlformatter.Formatter(indent="1", indent_char="\t", encoding_output="ISO-8859-1", preserve=["literal"])
+
+    files = FS.readFilesTypeFolder(outPath + 'sdf-generation/', fileType= '.sdf')
+    for el in files:
+        
+        FS.writeFile(data=str(formatter.format_file(outPath + 'sdf-generation/' + el) , 'utf-8'), filePath=outPath + 'sdf-generation/', fileName=el)
+    
+    
+

sdf/mocks/Cube1.json

@@ -0,0 +1,18 @@
+{
+    "name": "Cube1",
+    "ixx": "16.66666666666667",
+    "ixy": "0.0",
+    "ixz": "0.0",
+    "iyy": "16.66666666666667",
+    "izz": "16.66666666666667",
+    "massSDF": "0.9999999999999998",
+    "posX": "0.0",
+    "posY": "-0.015",
+    "posZ": "0.0",
+    "eulerX": "0.0",
+    "eulerY": "0.0",
+    "eulerZ": "0.0",
+    "iyz": "0.0",
+    "stl": "/meshes/Cube1.stl",
+    "link": "1554"
+}

sdf/mocks/Cube2.json

@@ -0,0 +1,18 @@
+{
+    "name": "Cube2",
+    "ixx": "16.66666666666667",
+    "ixy": "0.0",
+    "ixz": "-3.637978807091713e-15",
+    "iyy": "16.66666666666667",
+    "izz": "16.66666666666667",
+    "massSDF": "0.9999999999999998",
+    "posX": "0.0",
+    "posY": "-0.009",
+    "posZ": "0.01",
+    "eulerX": "0.0",
+    "eulerY": "0.0",
+    "eulerZ": "0.0",
+    "iyz": "-3.637978807091713e-15",
+    "stl": "/meshes/Cube2.stl",
+    "link": "8838"
+}

sdf/mocks/sdf/include.sdf

@@ -0,0 +1,4 @@
+<include>
+        <name>{name}</name>
+        <uri>{uri}</uri>
+</include>

sdf/mocks/sdf/include_pose.sdf

@@ -0,0 +1,5 @@
+  <include>
+    <name>{name}</name>
+    <uri>{uri}</uri>
+    <pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
+  </include>

sdf/mocks/sdf/joint_fixed.sdf

@@ -0,0 +1,7 @@
+<joint name="{name}" type="fixed">
+    <parent>base_link</parent>
+    <child>{child}::{child}</child>
+    <pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
+</joint>
+ 
+ 

sdf/mocks/sdf/link.sdf

@@ -0,0 +1,36 @@
+<link name="{name}">
+  <pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
+  <inertial>
+    <pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
+    <inertia>
+      <ixx>{ixx}</ixx>
+      <ixy>{ixy}</ixy>
+      <ixz>{ixz}</ixz>
+      <iyy>{iyy}</iyy>
+      <iyz>{iyz}</iyz>
+      <izz>{izz}</izz>
+    </inertia>
+    <mass>{massSDF}</mass>
+  </inertial>
+  <collision name="collision">
+    <geometry>
+      <mesh>
+        <uri>model:/{stl}</uri>
+      </mesh>
+    </geometry>
+  </collision>
+  <visual name="visual">
+    <geometry>
+      <mesh>
+        <uri>model:/{stl}</uri>
+      </mesh>
+    </geometry>
+      <surface>
+          <friction>
+            <ode>
+              <mu>{friction}</mu>
+            </ode>
+          </friction>
+      </surface>
+  </visual>
+</link>

sdf/mocks/sdf/model.config

@@ -0,0 +1,5 @@
+
+<?xml version="1.0" ?>
+<model>
+  <sdf version="1.5">model.sdf</sdf>
+</model>

sdf/mocks/sdf/model.sdf

@@ -0,0 +1,27 @@
+<?xml version='1.0'?>
+<sdf version="1.4">
+  <model name="{name}">
+    <link name="{name}">
+    <gravity>0</gravity>
+      <collision name="collision">
+        <mesh>
+          <uri>model:/{stl}</uri>
+        </mesh>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model:/{stl}</uri>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>{friction}</mu>
+            </ode>
+          </friction>
+        </surface>
+      </visual>
+  </link>
+  </model>
+</sdf>

sdf/mocks/sdf/world.sdf

@@ -0,0 +1,71 @@
+<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'/>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>true</shadows>
+    </scene>
+    <model name='ground_plane'>
+      <static>true</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </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>
+        <pose>0 0 0 0 -0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 -0 0</pose>
+          <mass>1</mass>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <enable_wind>false</enable_wind>
+      </link>
+      <pose>0 0 0 0 -0 0</pose>
+      <self_collide>false</self_collide>
+    </model>
+    
+</world>
+</sdf>

sdf/requirements.txt

@@ -0,0 +1 @@
+argparse

sdf/src/model/sdf_geometry.py

@@ -0,0 +1,161 @@
+import os
+from helper.fs import FS
+
+from src.model.sdf_join import SdfJoin
+import typing
+import uuid
+
+
+def from_str(x):
+    assert isinstance(x, str)
+    return x
+
+
+def from_none(x):
+    assert x is None
+    return x
+
+
+def from_union(fs, x):
+    for f in fs:
+        try:
+            return f(x)
+        except:
+            pass
+    assert False
+
+
+def to_class(c, x):
+    assert isinstance(x, c)
+    return x.to_dict()
+
+
+class SdfGeometryModel:
+    def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction):
+        self.name = name
+        self.ixx = ixx
+        self.ixy = ixy
+        self.ixz = ixz
+        self.iyy = iyy
+        self.izz = izz
+        self.massSDF = massSDF
+        self.posX = posX
+        self.posY = posY
+        self.posZ = posZ
+        self.eulerX = eulerX
+        self.eulerY = eulerY
+        self.eulerZ = eulerZ
+        self.iyz = iyz
+        self.stl = stl
+        self.link = link
+        self.friction = friction
+
+    @staticmethod
+    def from_dict(obj):
+        assert isinstance(obj, dict)
+        name = from_union([from_str, from_none], obj.get("name"))
+        ixx = from_union([from_str, from_none], obj.get("ixx"))
+        ixy = from_union([from_str, from_none], obj.get("ixy"))
+        ixz = from_union([from_str, from_none], obj.get("ixz"))
+        iyy = from_union([from_str, from_none], obj.get("iyy"))
+        izz = from_union([from_str, from_none], obj.get("izz"))
+        massSDF = from_union([from_str, from_none], obj.get("massSDF"))
+        posX = from_union([from_str, from_none], obj.get("posX"))
+        posY = from_union([from_str, from_none], obj.get("posY"))
+        posZ = from_union([from_str, from_none], obj.get("posZ"))
+        eulerX = from_union([from_str, from_none], obj.get("eulerX"))
+        eulerY = from_union([from_str, from_none], obj.get("eulerY"))
+        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'))
+        friction = from_union([from_str, from_none], obj.get("friction"))
+
+        return SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction)
+
+    def to_dict(self):
+        result = {}
+        if self.name is not None:
+            result["name"] = from_union([from_str, from_none], self.name)
+        if self.ixx is not None:
+            result["ixx"] = from_union([from_str, from_none], self.ixx)
+        if self.ixy is not None:
+            result["ixy"] = from_union([from_str, from_none], self.ixy)
+        if self.ixz is not None:
+            result["ixz"] = from_union([from_str, from_none], self.ixz)
+        if self.iyy is not None:
+            result["iyy"] = from_union([from_str, from_none], self.iyy)
+        if self.izz is not None:
+            result["izz"] = from_union([from_str, from_none], self.izz)
+        if self.massSDF is not None:
+            result["massSDF"] = from_union([from_str, from_none], self.massSDF)
+        if self.posX is not None:
+            result["posX"] = from_union([from_str, from_none], self.posX)
+        if self.posY is not None:
+            result["posY"] = from_union([from_str, from_none], self.posY)
+        if self.posZ is not None:
+            result["posZ"] = from_union([from_str, from_none], self.posZ)
+        if self.eulerX is not None:
+            result["eulerX"] = from_union([from_str, from_none], self.eulerX)
+        if self.eulerY is not None:
+            result["eulerY"] = from_union([from_str, from_none], self.eulerY)
+        if self.eulerZ is not None:
+            result["eulerZ"] = from_union([from_str, from_none], self.eulerZ)
+        if self.iyz is not None:
+            result["iyz"] = from_union([from_str, from_none], self.iyz)
+        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)
+        if self.friction is not None:
+            result["friction"] = from_union([from_str, from_none], self.eulerZ)
+        return result
+
+    def toJSON(self) -> str:
+        return str(self.to_dict()).replace('\'', '"')
+
+    def toSDF(self):
+        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+                           + '/../../mocks/sdf/model.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}',self.friction)
+
+    def toSdfLink(self):
+
+        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+                           + '/../../mocks/sdf/link.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}',self.friction)
+    def includeLink(self, pose = False):
+        if(pose == False):
+             return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+                           + '/../../mocks/sdf/include.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name)
+        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+                           + '/../../mocks/sdf/include_pose.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz)
+        
+    def generateSDFatJoinFixed(self, sdfModels: list['SdfGeometryModel']):
+        sdf = '\n<model name="assembly">\n'
+        sdf+=  '    <link name="base_link">\n'
+        sdf += "        <pose>0 0 0 0 0 0</pose>\n"
+        sdf+= "     </link>\n"
+        
+        link = sdf + self.includeLink(pose=True)
+        if sdfModels.__len__() == 0:
+            return link
+        endTagLinkInc = link.__len__()
+        beginSDF = link[0: endTagLinkInc]
+
+
+        sdfJoin = beginSDF + '\n'
+       
+        for el in sdfModels:
+            if el.name != self.name:
+                sdfJoin += el.includeLink(pose=True) + '\n'
+                
+        endSDF = link[endTagLinkInc:link.__len__()]
+
+        for el in sdfModels:
+            if el.name != self.name:
+                sdfJoin += SdfJoin(name=str(uuid.uuid4()),
+                                   parent=self.name, child=el.name,modelAt=el).toSDF() + '\n'
+
+        sdfJoin += endSDF
+        sdfJoin += '</model>'
+        return sdfJoin
+    

sdf/src/model/sdf_join.py

@@ -0,0 +1,16 @@
+from helper.fs import FS
+import os
+
+ 
+
+class SdfJoin:
+
+    def __init__(self, name, parent, modelAt, child) -> None:
+        self.name = name
+        self.parent = parent
+        self.child = child
+        self.modelAt = modelAt
+        pass
+
+    def toSDF(self):
+        return (FS.readFile(os.path.dirname(os.path.realpath(__file__)) + '/../../mocks/sdf/joint_fixed.sdf')).replace('{name}', self.name,).replace('{parent}', self.parent).replace('{child}', self.child).replace('{posX}', self.modelAt.posX).replace('{posY}', self.modelAt.posY).replace('{posZ}', self.modelAt.posZ).replace('{eulerX}', self.modelAt.eulerX).replace('{eulerY}', self.modelAt.eulerY).replace('{eulerZ}', self.modelAt.eulerZ).replace('{ixx}', self.modelAt.ixx).replace('{ixy}', self.modelAt.ixy).replace('{ixz}', self.modelAt.ixz).replace('{iyy}', self.modelAt.iyy).replace('{iyz}', self.modelAt.iyz).replace('{izz}', self.modelAt.izz)

sdf/src/usecases/generate_world.py

@@ -0,0 +1,12 @@
+import os
+from helper.fs import FS
+
+class SdfGenerateWorldUseCase:
+    def call(assembly:str) -> str:
+        world = FS.readFile(os.path.dirname(os.path.realpath(__file__))
+                           + '/../../mocks/sdf/world.sdf')
+        beginWorld = world[0:world.find('</world') - 1]
+        endWorld = world[world.find('</world') - 1: world.__len__()]
+        
+        
+        return beginWorld + assembly + endWorld

sdf/src/usecases/sdf_sub_assembly_usecase.py

@@ -0,0 +1,45 @@
+from typing import Optional
+from src.model.sdf_geometry import SdfGeometryModel
+
+
+def listGetFirstValue(iterable, default=False, pred=None):
+    return next(filter(pred, iterable), default)
+
+
+def filterModels(filterModels: list[SdfGeometryModel], filterModelsDescription: list[str]):
+    models = []
+    for el in filterModelsDescription:
+        models.append(listGetFirstValue(filterModels, None, lambda x:  x.name == el))
+    return models
+
+
+class SdfSubAssemblyUseCase:
+
+    def call(self, sdfGeometryModels: list[SdfGeometryModel], assembly: list[str]):
+        asm = {}
+        generateSubAssemblyModels = self.generateSubAssembly(assembly)
+        inc = 0
+        for key, value in generateSubAssemblyModels.items():
+            inc += 1
+            if value['assembly'].__len__() != 0:
+
+                model: Optional[SdfGeometryModel] = listGetFirstValue(
+                    sdfGeometryModels, None, lambda x:  x.name == value['assembly'][0])
+                 
+                if model != None:
+                
+                    asm[key] = {"assembly": model.generateSDFatJoinFixed(filterModels(sdfGeometryModels,  value['assembly'])), "part": (
+                        listGetFirstValue(sdfGeometryModels, None, lambda x:  x.name == value['part'])).includeLink()}
+    
+        return asm
+
+    def generateSubAssembly(self, assembly: list[str]):
+        asm = {}
+        inc = 0
+        for el in assembly:
+            asm[str("asm" + str(inc))] = {
+                "part": el,
+                "assembly": assembly[0:inc],
+            }
+            inc += 1
+        return asm

setup.py

@@ -18,4 +18,4 @@ setuptools.setup(
     #},
     #python_requires='>=3.10',
     packages=setuptools.find_packages(),
-)
+)