Экспорт URDF из FreeCAD
This commit is contained in:
parent
7cadf0741f
commit
45e0d29ea0
13 changed files with 267 additions and 98 deletions
3
.gitignore
vendored
3
.gitignore
vendored
|
@ -112,3 +112,6 @@ install_plugin_cad.sh
|
||||||
*#
|
*#
|
||||||
.#*
|
.#*
|
||||||
\#*\#
|
\#*\#
|
||||||
|
Cube3
|
||||||
|
sdf-generation
|
||||||
|
p.json
|
|
@ -1,15 +1,15 @@
|
||||||
import importDAE
|
# import importDAE
|
||||||
import FreeCAD as App
|
import FreeCAD as App
|
||||||
from model.files_generator import FolderGenerator
|
from model.files_generator import FolderGenerator
|
||||||
from helper.is_solid import is_object_solid
|
from helper.is_solid import is_object_solid
|
||||||
|
import Mesh
|
||||||
|
|
||||||
class ExportUseCase:
|
class ExportUseCase:
|
||||||
def call(path):
|
def call(path):
|
||||||
meshes = {}
|
meshes = {}
|
||||||
for el in App.ActiveDocument.Objects:
|
for el in App.ActiveDocument.Objects:
|
||||||
if (is_object_solid(el)):
|
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')
|
'/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae')
|
||||||
meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
|
meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
|
||||||
'/' + el.Label + '.dae'
|
'/' + el.Label + '.dae'
|
||||||
|
|
|
@ -1,5 +1,6 @@
|
||||||
import os
|
import os
|
||||||
import json
|
import json
|
||||||
|
import typing
|
||||||
|
|
||||||
|
|
||||||
class FS:
|
class FS:
|
||||||
|
@ -21,3 +22,14 @@ class FS:
|
||||||
filter(lambda x: x[-fileType.__len__():] == fileType, os.listdir(pathFolder)))
|
filter(lambda x: x[-fileType.__len__():] == fileType, os.listdir(pathFolder)))
|
||||||
return filesJson
|
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
|
||||||
|
|
73
sdf/main.py
73
sdf/main.py
|
@ -1,71 +1,56 @@
|
||||||
import argparse
|
import argparse
|
||||||
import shutil
|
import shutil
|
||||||
from distutils.dir_util import copy_tree
|
|
||||||
import asyncio
|
|
||||||
from helper.fs import FS
|
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
|
from src.usecases.sdf_sub_assembly_usecase import SdfSubAssemblyUseCase
|
||||||
|
|
||||||
import os
|
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__":
|
if __name__ == "__main__":
|
||||||
parser = argparse.ArgumentParser()
|
parser = argparse.ArgumentParser()
|
||||||
parser.add_argument('--generationFolder', help='FreeCad generation folder')
|
parser.add_argument('--generationFolder', help='FreeCad generation folder')
|
||||||
parser.add_argument('--outPath', help='save SDF path')
|
parser.add_argument('--outPath', help='save SDF path')
|
||||||
parser.add_argument('--world', help='adding sdf world')
|
parser.add_argument('--world', help='adding sdf world')
|
||||||
|
parser.add_argument('--format', help='urdf,sdf,mujoco')
|
||||||
|
parser.add_argument('--stabilityCheck',
|
||||||
|
help='do i need to check the stability?')
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
|
|
||||||
if args.generationFolder == None or args.outPath == None:
|
if args.generationFolder == None or args.outPath == None:
|
||||||
parser.print_help()
|
parser.print_help()
|
||||||
outPath = args.outPath
|
outPath = args.outPath
|
||||||
|
|
||||||
geometryFiles = FS.readFilesTypeFolder(args.generationFolder + '/assets/')
|
geometryFiles = FS.readFilesTypeFolder(args.generationFolder + '/assets/')
|
||||||
assemblyStructure = FS.readJSON(
|
assemblyStructure = FS.readJSON(
|
||||||
args.generationFolder + '/step-structure.json')
|
args.generationFolder + '/step-structure.json')
|
||||||
|
|
||||||
sdfGeometryModels: list[SdfGeometryModel] = []
|
geometryModels: list[GeometryModel] = []
|
||||||
for el in geometryFiles:
|
for el in geometryFiles:
|
||||||
sdfGeometryModels.append(SdfGeometryModel.from_dict(
|
geometryModels.append(GeometryModel.from_dict(
|
||||||
FS.readJSON(args.generationFolder + '/assets/' + el)))
|
FS.readJSON(args.generationFolder + '/assets/' + el)))
|
||||||
sdfSubAssemblyUseCase = SdfSubAssemblyUseCase().call(
|
|
||||||
sdfGeometryModels, assemblyStructure,)
|
|
||||||
|
|
||||||
if os.path.exists(outPath + 'sdf-generation/'):
|
if os.path.exists(outPath + 'sdf-generation/'):
|
||||||
shutil.rmtree(path=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
|
||||||
|
)
|
||||||
|
|
30
sdf/mocks/urdf/model.urdf
Normal file
30
sdf/mocks/urdf/model.urdf
Normal file
|
@ -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>
|
16
sdf/src/model/asm.py
Normal file
16
sdf/src/model/asm.py
Normal file
|
@ -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)
|
2
sdf/src/model/enum.py
Normal file
2
sdf/src/model/enum.py
Normal file
|
@ -0,0 +1,2 @@
|
||||||
|
class Enum:
|
||||||
|
folderPath = 'sdf-generation/';
|
|
@ -30,7 +30,7 @@ def to_class(c, x):
|
||||||
return x.to_dict()
|
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):
|
def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction):
|
||||||
self.name = name
|
self.name = name
|
||||||
self.ixx = ixx
|
self.ixx = ixx
|
||||||
|
@ -71,7 +71,7 @@ class SdfGeometryModel:
|
||||||
link = from_union([from_str, from_none], obj.get('link'))
|
link = from_union([from_str, from_none], obj.get('link'))
|
||||||
friction = from_union([from_str, from_none], obj.get("friction"))
|
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):
|
def to_dict(self):
|
||||||
result = {}
|
result = {}
|
||||||
|
@ -116,46 +116,50 @@ class SdfGeometryModel:
|
||||||
|
|
||||||
def toSDF(self):
|
def toSDF(self):
|
||||||
return FS.readFile(os.path.dirname(os.path.realpath(__file__))
|
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):
|
def toSdfLink(self):
|
||||||
|
|
||||||
return FS.readFile(os.path.dirname(os.path.realpath(__file__))
|
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__))
|
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)
|
+ '/../../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 = '\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 += " <pose>0 0 0 0 0 0</pose>\n"
|
||||||
sdf+= " </link>\n"
|
sdf += " </link>\n"
|
||||||
|
|
||||||
link = sdf + self.includeLink(pose=True)
|
link = sdf + self.includeLink(pose=True)
|
||||||
if sdfModels.__len__() == 0:
|
if sdfModels.__len__() == 0:
|
||||||
return link
|
return link
|
||||||
endTagLinkInc = link.__len__()
|
endTagLinkInc = link.__len__()
|
||||||
beginSDF = link[0: endTagLinkInc]
|
beginSDF = link[0: endTagLinkInc]
|
||||||
|
|
||||||
|
|
||||||
sdfJoin = beginSDF + '\n'
|
sdfJoin = beginSDF + '\n'
|
||||||
|
|
||||||
for el in sdfModels:
|
for el in sdfModels:
|
||||||
if el.name != self.name:
|
if el.name != self.name:
|
||||||
sdfJoin += el.includeLink(pose=True) + '\n'
|
sdfJoin += el.includeLink(pose=True) + '\n'
|
||||||
|
|
||||||
endSDF = link[endTagLinkInc:link.__len__()]
|
endSDF = link[endTagLinkInc:link.__len__()]
|
||||||
|
|
||||||
for el in sdfModels:
|
for el in sdfModels:
|
||||||
if el.name != self.name:
|
if el.name != self.name:
|
||||||
sdfJoin += SdfJoin(name=str(uuid.uuid4()),
|
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 += endSDF
|
||||||
sdfJoin += '</model>'
|
sdfJoin += '</model>'
|
||||||
return sdfJoin
|
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)
|
||||||
|
|
||||||
|
|
15
sdf/src/usecases/formatter_usecase.py
Normal file
15
sdf/src/usecases/formatter_usecase.py
Normal file
|
@ -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)
|
12
sdf/src/usecases/sdf_generate_world_usecase.py
Normal file
12
sdf/src/usecases/sdf_generate_world_usecase.py
Normal file
|
@ -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
|
|
@ -1,21 +1,24 @@
|
||||||
|
import os
|
||||||
from typing import Optional
|
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 = {}
|
asm = {}
|
||||||
generateSubAssemblyModels = self.generateSubAssembly(assembly)
|
generateSubAssemblyModels = self.generateSubAssembly(assembly)
|
||||||
inc = 0
|
inc = 0
|
||||||
|
@ -23,23 +26,32 @@ class SdfSubAssemblyUseCase:
|
||||||
inc += 1
|
inc += 1
|
||||||
if value['assembly'].__len__() != 0:
|
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)
|
||||||
|
|
36
sdf/src/usecases/stability_check_usecase.py
Normal file
36
sdf/src/usecases/stability_check_usecase.py
Normal file
|
@ -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()
|
42
sdf/src/usecases/urdf_sub_assembly_usecase.py
Normal file
42
sdf/src/usecases/urdf_sub_assembly_usecase.py
Normal file
|
@ -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():
|
Loading…
Add table
Add a link
Reference in a new issue