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

Экспорт URDF из FreeCAD

Browse source
This commit is contained in:
IDONTSUDO 2023-04-24 19:42:48 +00:00 • committed by Igor Brylyov
parent 7cadf0741f
commit 45e0d29ea0
13 changed files with 267 additions and 98 deletions
Download patch file Download diff file Expand all files Collapse all files

3
.gitignore vendored
View file

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

6
cg/freecad/Frames/usecases/export_usecase.py
View file

@ -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'

12
sdf/helper/fs.py
View file

@ -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

73
sdf/main.py
View file

@ -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
)

30
sdf/mocks/urdf/model.urdf Normal file
View 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
View 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
View file

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

40
sdf/src/model/sdf_geometry.py
View file

@ -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<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)

15
sdf/src/usecases/formatter_usecase.py Normal file
View 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
View 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

78
sdf/src/usecases/sdf_sub_assembly_usecase.py
View file

@ -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):
return next(filter(pred, iterable), default)
class SdfSubAssemblyUseCase(Assembly):
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]):
def call(self, geometryModels: list[GeometryModel], assembly: list[str], outPath: str, generationFolder: str, world: bool):
asm = {}
generateSubAssemblyModels = self.generateSubAssembly(assembly)
inc = 0
@ -23,23 +26,32 @@ class SdfSubAssemblyUseCase:
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
model: Optional[GeometryModel] = listGetFirstValue(
geometryModels, None, lambda x: x.name == value['assembly'][0])
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
if model != None:
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)
dirPath = outPath + Enum.folderPath
for el in geometryModels:
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
View 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
View 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():