robossembler/framework
7
0
Fork 0
Code Issues 21 Pull requests Projects Releases Packages Wiki Activity Actions

Add PDDL, 3D-assets & SDF-URDF generator from Blender Scene Config

Browse source
This commit is contained in:
IDONTSUDO 2023-12-17 13:58:43 +00:00 • committed by Igor Brylyov
parent b77687ea14
commit e305d486f2
41 changed files with 2793 additions and 664 deletions
Download patch file Download diff file Expand all files Collapse all files

20
asp/helper/fs.py
View file

@ -8,18 +8,21 @@ class FS:
return json.loads((open(path)).read())
def writeFile(data, filePath, fileName):
file_to_open = filePath + fileName
f = open(file_to_open, 'w', )
f = open(file_to_open, "w", encoding="utf-8", errors="ignore")
f.write(data)
def readFile(path:str):
f.close()
def readFile(path: str):
return open(path).read()
def readFilesTypeFolder(pathFolder: str, fileType = '.json'):
def readFilesTypeFolder(pathFolder: str, fileType=".json"):
filesJson = list(
filter(lambda x: x[-fileType.__len__():] == fileType, os.listdir(pathFolder)))
filter(
lambda x: x[-fileType.__len__() :] == fileType, os.listdir(pathFolder)
)
)
return filesJson
@ -30,6 +33,5 @@ def listGetFirstValue(iterable, default=False, pred=None):
def filterModels(filterModels, filterModelsDescription: list[str]):
models = []
for el in filterModelsDescription:
models.append(listGetFirstValue(
filterModels, None, lambda x: x.name == el))
models.append(listGetFirstValue(filterModels, None, lambda x: x.name == el))
return models

42
asp/main.py
View file

@ -1,45 +1,53 @@
import argparse
import shutil
from src.model.enum import Enum
from helper.fs import FS
from src.usecases.urdf_sub_assembly_usecase import UrdfSubAssemblyUseCase
from src.model.sdf_geometry import GeometryModel
from src.usecases.sdf_sub_assembly_usecase import SdfSubAssemblyUseCase
import os
from pathlib import Path
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("--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")
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')
geometryFiles = FS.readFilesTypeFolder(args.generationFolder + "/assets/")
assemblyStructure = FS.readJSON(args.generationFolder + "/step-structure.json")
geometryModels: list[GeometryModel] = []
for el in geometryFiles:
geometryModels.append(GeometryModel.from_dict(
FS.readJSON(args.generationFolder + '/assets/' + el)))
# if os.path.exists(outPath + 'sdf-generation/'):
# shutil.rmtree(path=outPath + 'sdf-generation/')
geometryModels.append(
GeometryModel.from_dict(
FS.readJSON(args.generationFolder + "/assets/" + el)
)
)
if os.path.exists(outPath + Enum.folderPath):
shutil.rmtree(outPath + Enum.folderPath)
Path(outPath + Enum.folderPath).mkdir(parents=True, exist_ok=True)
if (args.format == 'sdf'):
if args.format == "sdf":
SdfSubAssemblyUseCase().call(
geometryModels=geometryModels, assembly=assemblyStructure,
geometryModels=geometryModels,
assembly=assemblyStructure,
world=args.world,
generationFolder=args.generationFolder,
outPath=args.outPath
outPath=args.outPath,
)
if (args.format == 'urdf'):
if args.format == "urdf":
UrdfSubAssemblyUseCase().call(
geometryModels=geometryModels, assembly=assemblyStructure,
geometryModels=geometryModels,
assembly=assemblyStructure,
world=args.world,
generationFolder=args.generationFolder,
outPath=args.outPath
outPath=args.outPath,
)

13
asp/mocks/sdf/world.sdf
View file

@ -1,14 +1,7 @@
<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'/>
@ -66,6 +59,6 @@
<pose>0 0 0 0 -0 0</pose>
<self_collide>false</self_collide>
</model>
</world>
</sdf>

2
asp/src/model/enum.py
View file

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

208
asp/src/model/sdf_geometry.py
View file

@ -34,7 +34,29 @@ DELIMITER_SCALE = 10000
class GeometryModel:
def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction, centerMassX, centerMassY, centerMassZ):
def __init__(
self,
name,
ixx,
ixy,
ixz,
iyy,
izz,
massSDF,
posX,
posY,
posZ,
eulerX,
eulerY,
eulerZ,
iyz,
stl,
link,
friction,
centerMassX,
centerMassY,
centerMassZ,
):
self.name = name
self.ixx = ixx
self.ixy = ixy
@ -74,12 +96,33 @@ class GeometryModel:
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'))
link = from_union([from_str, from_none], obj.get("link"))
friction = from_union([from_str, from_none], obj.get("friction"))
centerMassX = from_union([from_str, from_none], obj.get("centerMassX"))
centerMassY = from_union([from_str, from_none], obj.get("centerMassY"))
centerMassZ = from_union([from_str, from_none], obj.get("centerMassZ"))
return GeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction, centerMassX, centerMassY, centerMassZ)
return GeometryModel(
name,
ixx,
ixy,
ixz,
iyy,
izz,
massSDF,
posX,
posY,
posZ,
eulerX,
eulerY,
eulerZ,
iyz,
stl,
link,
friction,
centerMassX,
centerMassY,
centerMassZ,
)
def to_dict(self):
result = {}
@ -114,40 +157,112 @@ class GeometryModel:
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)
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)
if self.centerMassX is not None:
result['centerMassX'] = from_union(
[from_str, from_none], self.centerMassX)
result["centerMassX"] = from_union([from_str, from_none], self.centerMassX)
if self.centerMassY is not None:
result['centerMassY'] = from_union(
[from_str, from_none], self.centerMassY)
result["centerMassY"] = from_union([from_str, from_none], self.centerMassY)
if self.centerMassZ is not None:
result['centerMassZ'] = from_union(
[from_str, from_none], self.centerMassZ)
result["centerMassZ"] = from_union([from_str, from_none], self.centerMassZ)
return result
def toJSON(self) -> str:
return str(self.to_dict()).replace('\'', '"')
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)
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)
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)
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['GeometryModel']):
def generateSDFatJoinFixed(self, sdfModels: list["GeometryModel"]):
sdf = '\n<model name="assembly">\n'
sdf += ' <link name="base_link">\n'
sdf += " <pose>0 0 0 0 0 0</pose>\n"
@ -157,25 +272,56 @@ class GeometryModel:
if sdfModels.__len__() == 0:
return link
endTagLinkInc = link.__len__()
beginSDF = link[0: endTagLinkInc]
beginSDF = link[0:endTagLinkInc]
sdfJoin = beginSDF + '\n'
sdfJoin = beginSDF + "\n"
for el in sdfModels:
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:
if el.name != self.name:
sdfJoin += SdfJoin(name=str(uuid.uuid4()),
parent=self.name, child=el.name, modelAt=el).toSDF() + '\n'
sdfJoin += (
SdfJoin(
name=str(uuid.uuid4()),
parent=self.name,
child=el.name,
modelAt=el,
).toSDF()
+ "\n"
)
sdfJoin += endSDF
sdfJoin += '</model>'
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).replace('{centerMassX}', self.centerMassX).replace('{centerMassY}', self.centerMassY).replace('{centerMassZ}', self.centerMassZ)
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)
.replace("{centerMassX}", self.centerMassX)
.replace("{centerMassY}", self.centerMassY)
.replace("{centerMassZ}", self.centerMassZ)
)

22
asp/src/usecases/generate_world.py
View file

@ -1,12 +1,18 @@
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
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
class GeometryValidateUseCase:
def call(geometry) -> str:
return

71
asp/src/usecases/sdf_sub_assembly_usecase.py
View file

@ -9,49 +9,70 @@ 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'
SDF_FILE_FORMAT = ".sdf"
CONFIG_PATH = (
os.path.dirname(os.path.realpath(__file__)) + "/../../mocks/sdf/model.config"
)
class SdfSubAssemblyUseCase(Assembly):
def call(self, geometryModels: list[GeometryModel], assembly: list[str], outPath: str, generationFolder: str, world: bool):
def call(
self,
geometryModels: list[GeometryModel],
assembly: list[str],
outPath: str,
generationFolder: str,
world: bool,
):
asm = {}
generateSubAssemblyModels = self.generateSubAssembly(assembly)
inc = 0
for key, value in generateSubAssemblyModels.items():
inc += 1
if value['assembly'].__len__() != 0:
if value["assembly"].__len__() != 0:
model: Optional[GeometryModel] = listGetFirstValue(
geometryModels, 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(geometryModels, value["assembly"])
),
"part": (
listGetFirstValue(
geometryModels, None, lambda x: x.name == value["part"]
)
).includeLink(),
}
asm[key] = {"assembly": model.generateSDFatJoinFixed(filterModels(geometryModels, value['assembly'])), "part": (
listGetFirstValue(geometryModels, None, lambda x: x.name == value['part'])).includeLink()}
self.copy(generationFolder=
generationFolder, format='/sdf', outPath=outPath)
self.copy(generationFolder=generationFolder, format="/sdf", outPath=outPath)
dirPath = outPath + Enum.folderPath
for el in geometryModels:
path = dirPath + el.name + '/'
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')
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)
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)
FS.writeFile(
data=SdfGenerateWorldUseCase.call(v["assembly"]),
filePath=dirPath,
fileName="/" + key + SDF_FILE_FORMAT,
)
FormatterUseCase.call(outPath=outPath, format=SDF_FILE_FORMAT)

21
asp/src/usecases/urdf_sub_assembly_usecase.py
View file

@ -6,14 +6,25 @@ import json
import re
URDF_FILE_FORMAT = '.urdf'
URDF_GENERATOR_FILE = 'urdf-generation' + '.json'
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):
def call(
self,
geometryModels: list[GeometryModel],
assembly: list[str],
outPath: str,
generationFolder: str,
world: bool,
):
dirPath = generationFolder + Enum.folderPath
asm = {}
for el in geometryModels:
asm[el.name] = el.toUrdf()
FS.writeFile(data=json.dumps(asm,indent=4),
fileName=URDF_GENERATOR_FILE, filePath=dirPath)
FS.writeFile(
data=json.dumps(asm, indent=4),
fileName=URDF_GENERATOR_FILE,
filePath=dirPath,
)