diff --git a/.gitignore b/.gitignore
index 3390209..3332cf1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -112,3 +112,6 @@ install_plugin_cad.sh
*#
.#*
\#*\#
+Cube3
+sdf-generation
+p.json
\ No newline at end of file
diff --git a/cg/freecad/Frames/usecases/export_usecase.py b/cg/freecad/Frames/usecases/export_usecase.py
index 519982a..bce09c6 100644
--- a/cg/freecad/Frames/usecases/export_usecase.py
+++ b/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'
diff --git a/sdf/helper/fs.py b/sdf/helper/fs.py
index a529801..0cdfb23 100644
--- a/sdf/helper/fs.py
+++ b/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
diff --git a/sdf/main.py b/sdf/main.py
index e4ee14f..893c97d 100644
--- a/sdf/main.py
+++ b/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.model.sdf_geometry import SdfGeometryModel
+from src.usecases.stability_check_usecase import StabilityCheckUseCase
+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
+ )
diff --git a/sdf/mocks/urdf/model.urdf b/sdf/mocks/urdf/model.urdf
new file mode 100644
index 0000000..8c210af
--- /dev/null
+++ b/sdf/mocks/urdf/model.urdf
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/sdf/src/model/asm.py b/sdf/src/model/asm.py
new file mode 100644
index 0000000..db07cb3
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/sdf/src/model/enum.py b/sdf/src/model/enum.py
new file mode 100644
index 0000000..88f24f8
--- /dev/null
+++ b/sdf/src/model/enum.py
@@ -0,0 +1,2 @@
+class Enum:
+ folderPath = 'sdf-generation/';
diff --git a/sdf/src/model/sdf_geometry.py b/sdf/src/model/sdf_geometry.py
index 866a7ff..055b859 100644
--- a/sdf/src/model/sdf_geometry.py
+++ b/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)
- 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)
+ + '/../../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']):
+
+ def generateSDFatJoinFixed(self, sdfModels: list['GeometryModel']):
sdf = '\n\n'
- sdf+= ' \n'
+ sdf += ' \n'
sdf += " 0 0 0 0 0 0\n"
- sdf+= " \n"
-
+ sdf += " \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 += ''
return sdfJoin
-
\ No newline at end of file
+
+ 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)
+
diff --git a/sdf/src/usecases/formatter_usecase.py b/sdf/src/usecases/formatter_usecase.py
new file mode 100644
index 0000000..fa0de3f
--- /dev/null
+++ b/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)
diff --git a/sdf/src/usecases/sdf_generate_world_usecase.py b/sdf/src/usecases/sdf_generate_world_usecase.py
new file mode 100644
index 0000000..96b842e
--- /dev/null
+++ b/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('