Экспорт URDF из FreeCAD

.gitignore

@@ -112,3 +112,6 @@ install_plugin_cad.sh
 *#
 .#*
 \#*\#
+Cube3
+sdf-generation
+p.json

cg/freecad/Frames/usecases/export_usecase.py

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

sdf/helper/fs.py

@@ -1,5 +1,6 @@
 import os
 import json
+import typing
 
 
 class FS:
@@ -21,3 +22,14 @@ class FS:
             filter(lambda x: x[-fileType.__len__():] == fileType,  os.listdir(pathFolder)))
         return filesJson
 
+
+def listGetFirstValue(iterable, default=False, pred=None):
+    return next(filter(pred, iterable), default)
+
+
+def filterModels(filterModels, filterModelsDescription: list[str]):
+    models = []
+    for el in filterModelsDescription:
+        models.append(listGetFirstValue(
+            filterModels, None, lambda x:  x.name == el))
+    return models

sdf/main.py

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

sdf/mocks/urdf/model.urdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<robot name="{name}">
+  <link name="baseLink">
+    <contact>
+      <friction_anchor/>
+      <lateral_friction value="0.3"/>
+      <rolling_friction value="0.0"/>
+      <contact_cfm value="0.0"/>
+      <contact_erp value="1.0"/>
+    </contact>
+    <inertial>
+       <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="{massSDF}"/>
+       <inertia ixx="{ixx}" ixy="{ixy}" ixz="{ixz}" iyy="{iyy}" iyz="{iyz}" izz="{izz}"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="{stl}" scale="1 1 1"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1. 1. 1. 1."/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+    	 	<mesh filename="{stl}" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

sdf/src/model/asm.py

@@ -0,0 +1,16 @@
+from distutils.dir_util import copy_tree
+from src.model.enum import Enum
+
+class Assembly:
+    def generateSubAssembly(self, assembly: list[str]):
+        asm = {}
+        inc = 0
+        for el in assembly:
+            asm[str("asm" + str(inc))] = {
+                "part": el,
+                "assembly": assembly[0:inc],
+            }
+            inc += 1
+        return asm
+    def copy(self,generationFolder,format,outPath  ):
+        copy_tree(generationFolder + format, outPath + Enum.folderPath)

sdf/src/model/enum.py

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

sdf/src/model/sdf_geometry.py

@@ -30,7 +30,7 @@ def to_class(c, x):
     return x.to_dict()
 
 
-class SdfGeometryModel:
+class GeometryModel:
     def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction):
         self.name = name
         self.ixx = ixx
@@ -71,7 +71,7 @@ class SdfGeometryModel:
         link = from_union([from_str, from_none], obj.get('link'))
         friction = from_union([from_str, from_none], obj.get("friction"))
 
-        return SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction)
+        return GeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction)
 
     def to_dict(self):
         result = {}
@@ -116,46 +116,50 @@ class SdfGeometryModel:
 
     def toSDF(self):
         return FS.readFile(os.path.dirname(os.path.realpath(__file__))
-                           + '/../../mocks/sdf/model.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}',self.friction)
+                           + '/../../mocks/sdf/model.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}', self.friction)
 
     def toSdfLink(self):
 
         return FS.readFile(os.path.dirname(os.path.realpath(__file__))
-                           + '/../../mocks/sdf/link.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}',self.friction)
+                           + '/../../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):
+    def includeLink(self, pose=False):
-             return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+        if (pose == False):
-                           + '/../../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.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name)
         return FS.readFile(os.path.dirname(os.path.realpath(__file__))
                            + '/../../mocks/sdf/include_pose.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz)
-        
+
-    def generateSDFatJoinFixed(self, sdfModels: list['SdfGeometryModel']):
+    def generateSDFatJoinFixed(self, sdfModels: list['GeometryModel']):
         sdf = '\n<model name="assembly">\n'
-        sdf+=  '    <link name="base_link">\n'
+        sdf += '    <link name="base_link">\n'
         sdf += "        <pose>0 0 0 0 0 0</pose>\n"
-        sdf+= "     </link>\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'
+                                   parent=self.name, child=el.name, modelAt=el).toSDF() + '\n'
 
         sdfJoin += endSDF
         sdfJoin += '</model>'
         return sdfJoin
-    
+
+    def toUrdf(self):
+        return FS.readFile(os.path.dirname(os.path.realpath(__file__))
+                           + '/../../mocks/urdf/model.urdf').replace('{name}', self.name).replace('{name}', self.name).replace('{uri}', '/' + self.name).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{stl}', '/' + self.stl).replace('{massSDF}', self.massSDF)
+        

sdf/src/usecases/formatter_usecase.py

@@ -0,0 +1,15 @@
+from src.model.enum import Enum
+import xmlformatter
+from helper.fs import FS
+
+
+class FormatterUseCase:
+    def call(outPath: str,   format: str):
+        formatter = xmlformatter.Formatter(
+            indent="1", indent_char="\t", encoding_output="ISO-8859-1", preserve=["literal"])
+
+        files = FS.readFilesTypeFolder(
+            outPath + Enum.folderPath, fileType=format)
+        for el in files:
+            FS.writeFile(data=str(formatter.format_file(outPath + Enum.folderPath + el),
+                         'utf-8'), filePath=outPath + Enum.folderPath, fileName=el)

sdf/src/usecases/sdf_generate_world_usecase.py

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

sdf/src/usecases/sdf_sub_assembly_usecase.py

@@ -1,21 +1,24 @@
+import os
 from typing import Optional
-from src.model.sdf_geometry import SdfGeometryModel
+from helper.fs import FS
+from helper.fs import filterModels, listGetFirstValue
+from src.model.asm import Assembly
+from src.model.enum import Enum
+from src.usecases.formatter_usecase import FormatterUseCase
+from src.usecases.sdf_generate_world_usecase import SdfGenerateWorldUseCase
+from src.model.sdf_geometry import GeometryModel
+from distutils.dir_util import copy_tree
+
+SDF_FILE_FORMAT = '.sdf'
+CONFIG_PATH = os.path.dirname(os.path.realpath(
+    __file__)) + '/../../mocks/sdf/model.config'
+
+ 
 
 
-def listGetFirstValue(iterable, default=False, pred=None):
+class SdfSubAssemblyUseCase(Assembly):
-    return next(filter(pred, iterable), default)
 
-
+    def call(self, geometryModels: list[GeometryModel], assembly: list[str], outPath: str, generationFolder: str, world: bool):
-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
@@ -23,23 +26,32 @@ class SdfSubAssemblyUseCase:
             inc += 1
             if value['assembly'].__len__() != 0:
 
-                model: Optional[SdfGeometryModel] = listGetFirstValue(
+                model: Optional[GeometryModel] = listGetFirstValue(
-                    sdfGeometryModels, 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(sdfGeometryModels,  value['assembly'])), "part": (
-                        listGetFirstValue(sdfGeometryModels, None, lambda x:  x.name == value['part'])).includeLink()}
-    
-        return asm
 
-    def generateSubAssembly(self, assembly: list[str]):
+                if model != None:
-        asm = {}
+
-        inc = 0
+                    asm[key] = {"assembly": model.generateSDFatJoinFixed(filterModels(geometryModels,  value['assembly'])), "part": (
-        for el in assembly:
+                        listGetFirstValue(geometryModels, None, lambda x:  x.name == value['part'])).includeLink()}
-            asm[str("asm" + str(inc))] = {
+
-                "part": el,
+        self.copy(generationFolder=
-                "assembly": assembly[0:inc],
+                  generationFolder, format='/sdf', outPath=outPath)
-            }
+        dirPath = outPath + Enum.folderPath
-            inc += 1
+        for el in geometryModels:
-        return asm
+            path = dirPath + el.name + '/'
+            os.makedirs(path)
+            FS.writeFile(data=el.toSDF(), filePath=path,
+                         fileName='/model' + SDF_FILE_FORMAT)
+            FS.writeFile(data=FS.readFile(CONFIG_PATH),
+                         filePath=path, fileName='/model' + '.config')
+
+        for key, v in asm.items():
+            FS.writeFile(data=v['assembly'], filePath=dirPath,
+                         fileName='/' + key + SDF_FILE_FORMAT)
+    
+        else:
+            for key, v in asm.items():
+                FS.writeFile(data=SdfGenerateWorldUseCase.call(v['assembly']), filePath=dirPath,
+                             fileName='/' + key + SDF_FILE_FORMAT)
+
+        FormatterUseCase.call(outPath=outPath, format=SDF_FILE_FORMAT)

sdf/src/usecases/stability_check_usecase.py

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

sdf/src/usecases/urdf_sub_assembly_usecase.py

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