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

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