Экспорт подсборок с мешами (SDF) и плана сборки (PDDL) из FreeCAD в виде архива zip
This commit is contained in:
parent
7f13c0056f
commit
e65236aab6
37 changed files with 1022 additions and 176 deletions
1
.gitignore
vendored
1
.gitignore
vendored
|
@ -104,6 +104,7 @@ ENV/
|
|||
*.blend1
|
||||
install_plugin_cad.sh
|
||||
.vscode
|
||||
.DS_Store
|
||||
|
||||
# emacs backup files
|
||||
~*
|
||||
|
|
|
@ -13,7 +13,7 @@
|
|||
# License along with this library. If not, see <http://www.gnu.org/licenses/>.
|
||||
import FreeCAD
|
||||
import Tools
|
||||
from usecases.asm4parser_usecase import Asm4StructureParseUseCase
|
||||
from scenarios.robossembler_freecad_export_scenario import RobossemblerFreeCadExportScenario
|
||||
|
||||
if FreeCAD.GuiUp:
|
||||
import FreeCADGui
|
||||
|
@ -311,8 +311,9 @@ Tools.spawnClassCommand("FrameCommand",
|
|||
{"Pixmap": str(os.path.join(icondir, "frame.svg")),
|
||||
"MenuText": "Make a free frame",
|
||||
"ToolTip": "Make a freestanding reference frame."})
|
||||
|
||||
Tools.spawnClassCommand("ASM4StructureParsing",
|
||||
Asm4StructureParseUseCase().initParse,
|
||||
RobossemblerFreeCadExportScenario().call,
|
||||
{"Pixmap": str(os.path.join(icondir, "assembly4.svg")),
|
||||
"MenuText": "Make a ASM4 parsing",
|
||||
"ToolTip": "Make a ASM4 1"})
|
||||
|
|
14
cg/freecad/Frames/helper/fs.py
Normal file
14
cg/freecad/Frames/helper/fs.py
Normal file
|
@ -0,0 +1,14 @@
|
|||
import os
|
||||
import json
|
||||
|
||||
|
||||
class FS:
|
||||
def readJSON(path: str):
|
||||
return json.loads((open(path)).read())
|
||||
|
||||
def writeFile(data, filePath, fileName):
|
||||
file_to_open = filePath + fileName
|
||||
|
||||
f = open(file_to_open, 'w', )
|
||||
|
||||
f.write(data)
|
18
cg/freecad/Frames/helper/is_solid.py
Normal file
18
cg/freecad/Frames/helper/is_solid.py
Normal file
|
@ -0,0 +1,18 @@
|
|||
import FreeCAD
|
||||
|
||||
|
||||
def is_object_solid(obj):
|
||||
"""If obj is solid return True"""
|
||||
if not isinstance(obj, FreeCAD.DocumentObject):
|
||||
return False
|
||||
|
||||
if not hasattr(obj, 'Shape'):
|
||||
return False
|
||||
|
||||
if not hasattr(obj.Shape, 'Solids'):
|
||||
return False
|
||||
|
||||
if len(obj.Shape.Solids) == 0:
|
||||
return False
|
||||
|
||||
return True
|
12
cg/freecad/Frames/model/files_generator.py
Normal file
12
cg/freecad/Frames/model/files_generator.py
Normal file
|
@ -0,0 +1,12 @@
|
|||
from enum import Enum
|
||||
|
||||
|
||||
class FilesGenerator(Enum):
|
||||
DETAIL = 'detail.json'
|
||||
ASSEMBLY = 'assembly.json'
|
||||
|
||||
|
||||
class FolderGenerator(Enum):
|
||||
MESHES = 'meshes'
|
||||
ASSETS = 'assets'
|
||||
SDF = 'sdf'
|
86
cg/freecad/Frames/model/geometry_part.py
Normal file
86
cg/freecad/Frames/model/geometry_part.py
Normal file
|
@ -0,0 +1,86 @@
|
|||
from typing import Any, TypeVar, Type, cast
|
||||
|
||||
|
||||
T = TypeVar("T")
|
||||
|
||||
|
||||
def from_float(x: Any) -> float:
|
||||
assert isinstance(x, (float, int)) and not isinstance(x, bool)
|
||||
return float(x)
|
||||
|
||||
|
||||
def to_float(x: Any) -> float:
|
||||
assert isinstance(x, float)
|
||||
return x
|
||||
|
||||
|
||||
def to_class(c: Type[T], x: Any) -> dict:
|
||||
assert isinstance(x, c)
|
||||
return cast(Any, x).to_dict()
|
||||
|
||||
|
||||
class Axis:
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
|
||||
def __init__(self, x: float, y: float, z: float) -> None:
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.z = z
|
||||
|
||||
@staticmethod
|
||||
def from_dict(obj: Any) -> 'Axis':
|
||||
assert isinstance(obj, dict)
|
||||
x = from_float(obj.get("x"))
|
||||
y = from_float(obj.get("y"))
|
||||
z = from_float(obj.get("z"))
|
||||
return Axis(x, y, z)
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
result: dict = {}
|
||||
result["x"] = to_float(self.x)
|
||||
result["y"] = to_float(self.y)
|
||||
result["z"] = to_float(self.z)
|
||||
return result
|
||||
|
||||
|
||||
class GeometryPart:
|
||||
euler: Axis
|
||||
position: Axis
|
||||
rotation: Axis
|
||||
center: Axis
|
||||
|
||||
def __init__(self, euler: Axis, position: Axis, rotation: Axis, center: Axis) -> None:
|
||||
self.euler = euler
|
||||
self.position = position
|
||||
self.rotation = rotation
|
||||
self.center = center
|
||||
|
||||
@staticmethod
|
||||
def from_dict(obj: Any) -> 'GeometryPart':
|
||||
assert isinstance(obj, dict)
|
||||
euler = Axis.from_dict(obj.get("euler"))
|
||||
position = Axis.from_dict(obj.get("position"))
|
||||
rotation = Axis.from_dict(obj.get("rotation"))
|
||||
center = Axis.from_dict(obj.get("center"))
|
||||
return GeometryPart(euler, position, rotation, center)
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
result: dict = {}
|
||||
result["euler"] = to_class(Axis, self.euler)
|
||||
result["position"] = to_class(Axis, self.position)
|
||||
result["rotation"] = to_class(Axis, self.rotation)
|
||||
result["center"] = to_class(Axis, self.center)
|
||||
return result
|
||||
|
||||
def toJson(self) -> str:
|
||||
return str(self.to_dict()).replace('\'', '"')
|
||||
|
||||
|
||||
def geometry_part_from_dict(s: Any) -> GeometryPart:
|
||||
return GeometryPart.from_dict(s)
|
||||
|
||||
|
||||
def geometry_part_to_dict(x: GeometryPart) -> Any:
|
||||
return to_class(GeometryPart, x)
|
107
cg/freecad/Frames/model/sdf_geometry_model.py
Normal file
107
cg/freecad/Frames/model/sdf_geometry_model.py
Normal file
|
@ -0,0 +1,107 @@
|
|||
import json
|
||||
|
||||
|
||||
def from_str(x):
|
||||
assert isinstance(x, str)
|
||||
return x
|
||||
|
||||
|
||||
def from_none(x):
|
||||
assert x is None
|
||||
return x
|
||||
|
||||
|
||||
def from_union(fs, x):
|
||||
for f in fs:
|
||||
try:
|
||||
return f(x)
|
||||
except:
|
||||
pass
|
||||
assert False
|
||||
|
||||
|
||||
def to_class(c, x):
|
||||
assert isinstance(x, c)
|
||||
return x.to_dict()
|
||||
|
||||
|
||||
class SdfGeometryModel:
|
||||
def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction):
|
||||
self.name = name
|
||||
self.ixx = ixx
|
||||
self.ixy = ixy
|
||||
self.ixz = ixz
|
||||
self.iyy = iyy
|
||||
self.izz = izz
|
||||
self.massSDF = massSDF
|
||||
self.posX = posX
|
||||
self.posY = posY
|
||||
self.posZ = posZ
|
||||
self.eulerX = eulerX
|
||||
self.eulerY = eulerY
|
||||
self.eulerZ = eulerZ
|
||||
self.iyz = iyz
|
||||
self.stl = stl
|
||||
self.friction = friction
|
||||
|
||||
@staticmethod
|
||||
def from_dict(obj):
|
||||
assert isinstance(obj, dict)
|
||||
name = from_union([from_str, from_none], obj.get("name"))
|
||||
ixx = from_union([from_str, from_none], obj.get("ixx"))
|
||||
ixy = from_union([from_str, from_none], obj.get("ixy"))
|
||||
ixz = from_union([from_str, from_none], obj.get("ixz"))
|
||||
iyy = from_union([from_str, from_none], obj.get("iyy"))
|
||||
izz = from_union([from_str, from_none], obj.get("izz"))
|
||||
massSDF = from_union([from_str, from_none], obj.get("massSDF"))
|
||||
posX = from_union([from_str, from_none], obj.get("posX"))
|
||||
posY = from_union([from_str, from_none], obj.get("posY"))
|
||||
posZ = from_union([from_str, from_none], obj.get("posZ"))
|
||||
eulerX = from_union([from_str, from_none], obj.get("eulerX"))
|
||||
eulerY = from_union([from_str, from_none], obj.get("eulerY"))
|
||||
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") )
|
||||
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,friction)
|
||||
|
||||
def to_dict(self):
|
||||
result = {}
|
||||
if self.name is not None:
|
||||
result["name"] = from_union([from_str, from_none], self.name)
|
||||
if self.ixx is not None:
|
||||
result["ixx"] = from_union([from_str, from_none], self.ixx)
|
||||
if self.ixy is not None:
|
||||
result["ixy"] = from_union([from_str, from_none], self.ixy)
|
||||
if self.ixz is not None:
|
||||
result["ixz"] = from_union([from_str, from_none], self.ixz)
|
||||
if self.iyy is not None:
|
||||
result["iyy"] = from_union([from_str, from_none], self.iyy)
|
||||
if self.izz is not None:
|
||||
result["izz"] = from_union([from_str, from_none], self.izz)
|
||||
if self.massSDF is not None:
|
||||
result["massSDF"] = from_union([from_str, from_none], self.massSDF)
|
||||
if self.posX is not None:
|
||||
result["posX"] = from_union([from_str, from_none], self.posX)
|
||||
if self.posY is not None:
|
||||
result["posY"] = from_union([from_str, from_none], self.posY)
|
||||
if self.posZ is not None:
|
||||
result["posZ"] = from_union([from_str, from_none], self.posZ)
|
||||
if self.eulerX is not None:
|
||||
result["eulerX"] = from_union([from_str, from_none], self.eulerX)
|
||||
if self.eulerY is not None:
|
||||
result["eulerY"] = from_union([from_str, from_none], self.eulerY)
|
||||
if self.eulerZ is not None:
|
||||
result["eulerZ"] = from_union([from_str, from_none], self.eulerZ)
|
||||
if self.iyz is not None:
|
||||
result["iyz"] = from_union([from_str, from_none], self.iyz)
|
||||
if self.stl is not None:
|
||||
result["stl"] = from_union([from_str, from_none], self.stl)
|
||||
if self.friction is not None:
|
||||
result["friction"] = from_union([from_str, from_none], self.eulerZ)
|
||||
return result
|
||||
|
||||
def toJSON(self) -> str:
|
||||
return str(self.to_dict()).replace('\'', '"')
|
||||
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
import FreeCAD
|
||||
from usecases.export_usecase import ExportUseCase
|
||||
from usecases.get_sdf_geometry_usecase import SdfGeometryUseCase
|
||||
from usecases.assembly_parse_usecase import AssemblyParseUseCase
|
||||
from usecases.geometry_usecase import GeometryUseCase
|
||||
from model.geometry_part import GeometryPart
|
||||
from model.files_generator import FolderGenerator
|
||||
from helper.fs import FS
|
||||
from PySide import QtGui
|
||||
import os
|
||||
import ImportGui
|
||||
import shutil
|
||||
|
||||
|
||||
class RobossemblerFreeCadExportScenario:
|
||||
|
||||
def call(self):
|
||||
|
||||
path = self.qtGuiFeature()
|
||||
if path == None:
|
||||
return
|
||||
self.systemHelper(path)
|
||||
|
||||
def qtGuiFeature(self):
|
||||
if FreeCAD.ActiveDocument == None:
|
||||
FreeCAD.Console.PrintError("No active document")
|
||||
|
||||
p = QtGui.QFileDialog.getExistingDirectoryUrl()
|
||||
path = p.path()
|
||||
|
||||
if path == '':
|
||||
return None
|
||||
return path
|
||||
|
||||
def systemHelper(self, path: str):
|
||||
|
||||
root_label = FreeCAD.ActiveDocument.RootObjects[0].Label
|
||||
directory = path + '/' + root_label
|
||||
|
||||
if not os.path.exists(directory):
|
||||
os.makedirs(directory)
|
||||
|
||||
__objs__ = FreeCAD.ActiveDocument.RootObjects
|
||||
|
||||
os.makedirs(directory + '/' + FolderGenerator.ASSETS.value)
|
||||
os.makedirs(directory + '/' + FolderGenerator.SDF.value)
|
||||
os.makedirs(directory + '/' + FolderGenerator.SDF.value + '/' + FolderGenerator.MESHES.value)
|
||||
f = open(directory + "/step-structure.json", "w")
|
||||
f.write(AssemblyParseUseCase().toJson())
|
||||
f.close()
|
||||
self.geometry(directory)
|
||||
|
||||
ImportGui.export(__objs__, directory + '/' + 'assembly.step')
|
||||
|
||||
shutil.make_archive(directory, 'zip', directory)
|
||||
|
||||
shutil.rmtree(directory)
|
||||
return True
|
||||
|
||||
def geometry(self, outPutsPath: str):
|
||||
meshesExportUseCase = ExportUseCase.call(outPutsPath)
|
||||
for el in SdfGeometryUseCase.call(meshesExportUseCase):
|
||||
FS.writeFile(el.toJSON(), outPutsPath +
|
||||
'/' + FolderGenerator.ASSETS.value + '/', el.name + '.json',)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,53 +0,0 @@
|
|||
import FreeCAD as App
|
||||
|
||||
class Asm4StructureParseUseCase:
|
||||
_parts = []
|
||||
_label = []
|
||||
|
||||
def getSubPartsLabel(self, group):
|
||||
groupLabel = []
|
||||
for el in group:
|
||||
if str(el) == '<Part::PartFeature>':
|
||||
groupLabel.append(el.Label)
|
||||
return groupLabel
|
||||
|
||||
def parseLabel(self, nextGroup, label, level=2, nextGroupParse=0):
|
||||
if nextGroup.__len__() == nextGroupParse:
|
||||
return
|
||||
else:
|
||||
groupParts = []
|
||||
|
||||
for el in nextGroup:
|
||||
if str(el) == '<App::Link object>':
|
||||
groupParts.append(el)
|
||||
|
||||
for el in groupParts:
|
||||
if str(el) == '<App::Link object>':
|
||||
label.append({
|
||||
"level": level,
|
||||
"attachedTo": el.AttachedTo.split('#'),
|
||||
"label": el.Label,
|
||||
"axis": self.getSubPartsLabel(el.Group)
|
||||
})
|
||||
|
||||
def initParse(self):
|
||||
|
||||
model = App.ActiveDocument.RootObjects[1]
|
||||
self._label.append({
|
||||
"level": 1,
|
||||
"attachedTo": "Parent Assembly",
|
||||
"label": model.Label,
|
||||
"axis": self.getSubPartsLabel(model.Group)
|
||||
})
|
||||
for parent in model.Group:
|
||||
if str(parent) == '<App::Link object>':
|
||||
self._label.append({
|
||||
"level": 1,
|
||||
"attachedTo": parent.AttachedTo.split('#'),
|
||||
"label": parent.Label,
|
||||
"axis": self.getSubPartsLabel(parent.Group)
|
||||
})
|
||||
print(self._label)
|
||||
|
||||
|
||||
|
42
cg/freecad/Frames/usecases/assembly_parse_usecase.py
Normal file
42
cg/freecad/Frames/usecases/assembly_parse_usecase.py
Normal file
|
@ -0,0 +1,42 @@
|
|||
import FreeCAD as App
|
||||
from helper.is_solid import is_object_solid
|
||||
|
||||
|
||||
class AssemblyParseUseCase:
|
||||
_parts = []
|
||||
_asm = []
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.initParse()
|
||||
pass
|
||||
|
||||
|
||||
|
||||
def initParse(self):
|
||||
for el in App.ActiveDocument.Objects:
|
||||
if(is_object_solid(el)):
|
||||
self._asm.append(el.Label)
|
||||
|
||||
def toJson(self):
|
||||
return str(self._asm).replace('\'', "\"")
|
||||
|
||||
def getSubPartsLink(self, group):
|
||||
groupLink = {}
|
||||
for el in group:
|
||||
if (is_object_solid(el)):
|
||||
if str(el.Shape).find('Solid') != -1:
|
||||
if groupLink.get(el.Label) == None:
|
||||
groupLink[el.Label] = []
|
||||
for i in el.Group:
|
||||
|
||||
if str(i).find('Pad') != -1:
|
||||
groupLink[el.Label].append(i)
|
||||
if groupLink.__len__() == 0:
|
||||
return None
|
||||
return groupLink
|
||||
|
||||
def getLinkedProperty(self):
|
||||
return self._asm
|
||||
|
||||
|
||||
|
16
cg/freecad/Frames/usecases/export_usecase.py
Normal file
16
cg/freecad/Frames/usecases/export_usecase.py
Normal file
|
@ -0,0 +1,16 @@
|
|||
import importDAE
|
||||
import FreeCAD as App
|
||||
from model.files_generator import FolderGenerator
|
||||
from helper.is_solid import is_object_solid
|
||||
|
||||
|
||||
class ExportUseCase:
|
||||
def call(path):
|
||||
meshes = {}
|
||||
for el in App.ActiveDocument.Objects:
|
||||
if (is_object_solid(el)):
|
||||
importDAE.export([el], path + '/' + FolderGenerator.SDF.value +
|
||||
'/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae')
|
||||
meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
|
||||
'/' + el.Label + '.dae'
|
||||
return meshes
|
54
cg/freecad/Frames/usecases/geometry_usecase.py
Normal file
54
cg/freecad/Frames/usecases/geometry_usecase.py
Normal file
|
@ -0,0 +1,54 @@
|
|||
|
||||
import FreeCAD as App
|
||||
from helper.is_solid import is_object_solid
|
||||
|
||||
|
||||
class GeometryUseCase:
|
||||
def call() -> dict:
|
||||
labels = []
|
||||
for el in App.ActiveDocument.Objects:
|
||||
|
||||
if is_object_solid(el):
|
||||
labels.append(el.Label)
|
||||
|
||||
geometry = {
|
||||
"euler": {
|
||||
"x": None,
|
||||
"y": None,
|
||||
"z": None
|
||||
},
|
||||
"position": {
|
||||
"x": None,
|
||||
"y": None,
|
||||
"z": None
|
||||
},
|
||||
"rotation": {
|
||||
"x": None,
|
||||
"y": None,
|
||||
"z": None
|
||||
},
|
||||
"center": {
|
||||
"x": None,
|
||||
"y": None,
|
||||
"z": None
|
||||
},
|
||||
|
||||
}
|
||||
|
||||
boundBox = el.Shape.BoundBox
|
||||
geometry["center"]["x"] = boundBox.Center.x
|
||||
geometry["center"]["y"] = boundBox.Center.y
|
||||
geometry["center"]["z"] = boundBox.Center.z
|
||||
geometry["position"]['x'] = boundBox.XMax
|
||||
geometry["position"]['y'] = boundBox.YMax
|
||||
geometry["position"]['z'] = boundBox.ZMax
|
||||
rotation = el.Placement.Rotation
|
||||
geometry["rotation"]['x'] = rotation.Axis.z
|
||||
geometry["rotation"]['y'] = rotation.Axis.y
|
||||
geometry["rotation"]['z'] = rotation.Axis.z
|
||||
euler = el.Placement.Rotation.toEuler()
|
||||
geometry["euler"]['x'] = euler[0]
|
||||
geometry["euler"]['y'] = euler[1]
|
||||
geometry["euler"]['z'] = euler[2]
|
||||
|
||||
return {"geometry": geometry, "labels": labels, "label": el.Label}
|
63
cg/freecad/Frames/usecases/get_sdf_geometry_usecase.py
Normal file
63
cg/freecad/Frames/usecases/get_sdf_geometry_usecase.py
Normal file
|
@ -0,0 +1,63 @@
|
|||
import FreeCAD as App
|
||||
from model.sdf_geometry_model import SdfGeometryModel
|
||||
|
||||
from helper.is_solid import is_object_solid
|
||||
|
||||
|
||||
class SdfGeometryUseCase:
|
||||
def call(stlPaths:dict) -> list[SdfGeometryModel]:
|
||||
materialSolid = {}
|
||||
for el in App.ActiveDocument.Objects:
|
||||
if str(el) == '<App::MaterialObjectPython object>':
|
||||
friction = el.Material.get('SlidingFriction')
|
||||
for i in el.References:
|
||||
materialSolid[i[0].Label] = friction
|
||||
geometry = []
|
||||
for el in App.ActiveDocument.Objects:
|
||||
if is_object_solid(el):
|
||||
com = el.Shape.CenterOfMass
|
||||
mass = el.Shape.Mass
|
||||
inertia = el.Shape.MatrixOfInertia
|
||||
pos = el.Shape.Placement
|
||||
inertia = el.Shape.MatrixOfInertia
|
||||
name = el.Label
|
||||
ixx = str(inertia.A11 / 1000000)
|
||||
ixy = str(inertia.A12 / 1000000)
|
||||
ixz = str(inertia.A13 / 1000000)
|
||||
iyy = str(inertia.A22 / 1000000)
|
||||
iyz = str(inertia.A23 / 1000000)
|
||||
izz = str(inertia.A33 / 1000000)
|
||||
massSDF = str(mass / 1000000)
|
||||
posX = str(pos.Base[0] / 1000000)
|
||||
posY = str(pos.Base[1] / 1000000)
|
||||
posZ = str(pos.Base[2] / 1000000)
|
||||
eulerX = str(pos.Rotation.toEuler()[0])
|
||||
eulerY = str(pos.Rotation.toEuler()[1])
|
||||
eulerZ = str(pos.Rotation.toEuler()[2])
|
||||
|
||||
geometry.append(
|
||||
SdfGeometryModel(
|
||||
stl=stlPaths.get(el.Label),
|
||||
name=name,
|
||||
ixx=ixx,
|
||||
ixz=ixz,
|
||||
ixy=ixy,
|
||||
iyy=iyy,
|
||||
iyz=iyz,
|
||||
izz=izz,
|
||||
massSDF=massSDF,
|
||||
posX=posX,
|
||||
posY=posY,
|
||||
posZ=posZ,
|
||||
eulerX=eulerX,
|
||||
eulerY=eulerY,
|
||||
eulerZ=eulerZ,
|
||||
friction=materialSolid.get(el.Label) or '',
|
||||
)
|
||||
)
|
||||
return geometry
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -12,6 +12,8 @@ class FS:
|
|||
f = open(file_to_open, 'w', )
|
||||
|
||||
f.write(data)
|
||||
def readFile(path):
|
||||
return open(path).read()
|
||||
|
||||
def readFile(path:str):
|
||||
return open(path).read()
|
||||
|
|
14
pddl/main.py
14
pddl/main.py
|
@ -2,25 +2,23 @@ import argparse
|
|||
from helper.fs import FS
|
||||
from src.model.asm4_structure import Asm4Structure
|
||||
|
||||
from src.usecases.asm4_to_assembly_use_case import Asm4ToAssemblyUseCase
|
||||
from src.usecases.assembly_to_pddl_use_case import AssemblyToPddlUseCase
|
||||
|
||||
|
||||
|
||||
# python3 main.py --stepStructurePath /Users/idontsudo/robo/Cube3/step-structure.json --outPath /Users/idontsudo/robo/Cube3/pddl/
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--asm4Path', help='asm4 json FreeCad')
|
||||
parser.add_argument('--stepStructurePath', help='json step by FreeCad')
|
||||
parser.add_argument('--outPath', help='save pddl path')
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.asm4Path == None or args.outPath == None:
|
||||
if args.stepStructurePath == None or args.outPath == None:
|
||||
parser.print_help()
|
||||
|
||||
data = FS.readJSON(args.asm4Path)
|
||||
asm4 = Asm4Structure.parse(data)
|
||||
asm4usecase = Asm4ToAssemblyUseCase().call(asm4)
|
||||
assemblyToPddlUseCase = AssemblyToPddlUseCase.call(assembly=asm4usecase['asm'],rootLabel=asm4usecase['rootLabel'])
|
||||
data = FS.readJSON(args.stepStructurePath)
|
||||
|
||||
assemblyToPddlUseCase = AssemblyToPddlUseCase.call(assembly=data,rootLabel=data[0])
|
||||
FS.writeFile(assemblyToPddlUseCase['problem'] ,args.outPath, 'problem.pddl')
|
||||
FS.writeFile(assemblyToPddlUseCase['domain'] ,args.outPath, 'domain.pddl')
|
||||
|
||||
|
|
|
@ -1,86 +0,0 @@
|
|||
[
|
||||
{
|
||||
"level": 1,
|
||||
"attachedTo": "Parent Assembly",
|
||||
"label": "Model",
|
||||
"axis": [
|
||||
"LCS_Origin"
|
||||
]
|
||||
},
|
||||
{
|
||||
"level": 1,
|
||||
"attachedTo": [
|
||||
"Parent Assembly",
|
||||
"LCS_Origin"
|
||||
],
|
||||
"label": "CubePart001",
|
||||
"axis": [
|
||||
"LCS_0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"level": 1,
|
||||
"attachedTo": [
|
||||
"CubePart001",
|
||||
"LCS_0"
|
||||
],
|
||||
"label": "CubePart002",
|
||||
"axis": [
|
||||
"LCS_0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"level": 1,
|
||||
"attachedTo": [
|
||||
"CubePart001",
|
||||
"LCS_0"
|
||||
],
|
||||
"label": "CubePart003",
|
||||
"axis": [
|
||||
"LCS_0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"level": 1,
|
||||
"attachedTo": [
|
||||
""
|
||||
],
|
||||
"label": "CubePart004",
|
||||
"axis": [
|
||||
"LCS_0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"level": 1,
|
||||
"attachedTo": [
|
||||
"CubePart001",
|
||||
"LCS_0"
|
||||
],
|
||||
"label": "CubePart005",
|
||||
"axis": [
|
||||
"LCS_0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"level": 1,
|
||||
"attachedTo": [
|
||||
"CubePart005",
|
||||
"LCS_0"
|
||||
],
|
||||
"label": "CubePart006",
|
||||
"axis": [
|
||||
"LCS_0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"level": 1,
|
||||
"attachedTo": [
|
||||
"CubePart006",
|
||||
"LCS_0"
|
||||
],
|
||||
"label": "CubePart007",
|
||||
"axis": [
|
||||
"LCS_0"
|
||||
]
|
||||
}
|
||||
]
|
1
pddl/mocks/step-mock.json
Normal file
1
pddl/mocks/step-mock.json
Normal file
|
@ -0,0 +1 @@
|
|||
["Cube3", "Cube1", "Cube2"]
|
|
@ -1,19 +0,0 @@
|
|||
from src.model.asm4_structure import Asm4Structure
|
||||
from typing import List
|
||||
|
||||
|
||||
|
||||
class Asm4ToAssemblyUseCase:
|
||||
_asm4Assembly = []
|
||||
def call(self,asm4:List[Asm4Structure] ):
|
||||
rootPart = asm4[0]
|
||||
self._asm4Assembly.append(rootPart.label)
|
||||
for el in asm4[1:asm4.__len__()]:
|
||||
self.parse(lead=el)
|
||||
return {'asm':self._asm4Assembly[1:self._asm4Assembly.__len__()],'rootLabel':rootPart.label}
|
||||
def parse(self,lead:Asm4Structure ):
|
||||
if lead.attached_to[0] == 'Parent Assembly':
|
||||
self._asm4Assembly.append(lead.label)
|
||||
for i in self._asm4Assembly:
|
||||
if i == lead.attached_to[0]:
|
||||
self._asm4Assembly.append(lead.label)
|
|
@ -7,6 +7,7 @@ import os
|
|||
|
||||
class AssemblyToPddlUseCase:
|
||||
def call(assembly: List[str], rootLabel: str):
|
||||
print(assembly)
|
||||
partType = UserType("part")
|
||||
assemblyType = UserType('assembly')
|
||||
|
||||
|
@ -36,8 +37,8 @@ class AssemblyToPddlUseCase:
|
|||
goal = Fluent(rootLabel)
|
||||
problem.add_goal(connected(objectsPartPddl[objectsPartPddl.__len__(
|
||||
) - 1], objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]),)
|
||||
|
||||
return {
|
||||
"problem": unified_planning.io.PDDLWriter(problem).get_problem(),
|
||||
'domain': FS.readFile(os.path.dirname(os.path.realpath(__file__)) + '/../../mocks' + '/domain.txt'),
|
||||
}
|
||||
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
import unittest
|
||||
from src.usecases.asm4_to_assembly_use_case import Asm4ToAssemblyUseCase
|
||||
from src.usecases.assembly_to_pddl_use_case import AssemblyToPddlUseCase
|
||||
from src.model.asm4_structure import Asm4Structure
|
||||
|
||||
|
@ -7,17 +6,16 @@ from helper.fs import FS
|
|||
import os
|
||||
|
||||
|
||||
mock = FS.readJSON(os.path.dirname(os.path.realpath(__file__)) + '/mocks/asm4-mock.json')
|
||||
mock = FS.readJSON(os.path.dirname(os.path.realpath(__file__)) + '/mocks/step-mock.json')
|
||||
|
||||
asm4 = Asm4Structure.parse(mock)
|
||||
|
||||
|
||||
|
||||
asm4usecase = Asm4ToAssemblyUseCase().call(asm4)
|
||||
assemblyToPddl = AssemblyToPddlUseCase.call(assembly=asm4usecase['asm'],rootLabel=asm4usecase['rootLabel'])
|
||||
assemblyToPddl = AssemblyToPddlUseCase.call(assembly=mock,rootLabel=mock[0])
|
||||
|
||||
|
||||
class TestStringMethods(unittest.TestCase):
|
||||
def test_problem(self):
|
||||
print(assemblyToPddl["problem"])
|
||||
self.assertIsInstance(assemblyToPddl["problem"], str)
|
||||
def test_domain(self):
|
||||
self.assertIsInstance(assemblyToPddl["domain"], str)
|
||||
|
|
23
sdf/helper/fs.py
Normal file
23
sdf/helper/fs.py
Normal file
|
@ -0,0 +1,23 @@
|
|||
import os
|
||||
import json
|
||||
|
||||
|
||||
class FS:
|
||||
def readJSON(path: str):
|
||||
return json.loads((open(path)).read())
|
||||
|
||||
def writeFile(data, filePath, fileName):
|
||||
|
||||
file_to_open = filePath + fileName
|
||||
|
||||
f = open(file_to_open, 'w', )
|
||||
|
||||
f.write(data)
|
||||
def readFile(path:str):
|
||||
return open(path).read()
|
||||
|
||||
def readFilesTypeFolder(pathFolder: str, fileType = '.json'):
|
||||
filesJson = list(
|
||||
filter(lambda x: x[-fileType.__len__():] == fileType, os.listdir(pathFolder)))
|
||||
return filesJson
|
||||
|
71
sdf/main.py
Normal file
71
sdf/main.py
Normal file
|
@ -0,0 +1,71 @@
|
|||
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.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
|
||||
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')
|
||||
|
||||
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] = []
|
||||
for el in geometryFiles:
|
||||
sdfGeometryModels.append(SdfGeometryModel.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)
|
||||
|
||||
|
||||
|
18
sdf/mocks/Cube1.json
Normal file
18
sdf/mocks/Cube1.json
Normal file
|
@ -0,0 +1,18 @@
|
|||
{
|
||||
"name": "Cube1",
|
||||
"ixx": "16.66666666666667",
|
||||
"ixy": "0.0",
|
||||
"ixz": "0.0",
|
||||
"iyy": "16.66666666666667",
|
||||
"izz": "16.66666666666667",
|
||||
"massSDF": "0.9999999999999998",
|
||||
"posX": "0.0",
|
||||
"posY": "-0.015",
|
||||
"posZ": "0.0",
|
||||
"eulerX": "0.0",
|
||||
"eulerY": "0.0",
|
||||
"eulerZ": "0.0",
|
||||
"iyz": "0.0",
|
||||
"stl": "/meshes/Cube1.stl",
|
||||
"link": "1554"
|
||||
}
|
18
sdf/mocks/Cube2.json
Normal file
18
sdf/mocks/Cube2.json
Normal file
|
@ -0,0 +1,18 @@
|
|||
{
|
||||
"name": "Cube2",
|
||||
"ixx": "16.66666666666667",
|
||||
"ixy": "0.0",
|
||||
"ixz": "-3.637978807091713e-15",
|
||||
"iyy": "16.66666666666667",
|
||||
"izz": "16.66666666666667",
|
||||
"massSDF": "0.9999999999999998",
|
||||
"posX": "0.0",
|
||||
"posY": "-0.009",
|
||||
"posZ": "0.01",
|
||||
"eulerX": "0.0",
|
||||
"eulerY": "0.0",
|
||||
"eulerZ": "0.0",
|
||||
"iyz": "-3.637978807091713e-15",
|
||||
"stl": "/meshes/Cube2.stl",
|
||||
"link": "8838"
|
||||
}
|
4
sdf/mocks/sdf/include.sdf
Normal file
4
sdf/mocks/sdf/include.sdf
Normal file
|
@ -0,0 +1,4 @@
|
|||
<include>
|
||||
<name>{name}</name>
|
||||
<uri>{uri}</uri>
|
||||
</include>
|
5
sdf/mocks/sdf/include_pose.sdf
Normal file
5
sdf/mocks/sdf/include_pose.sdf
Normal file
|
@ -0,0 +1,5 @@
|
|||
<include>
|
||||
<name>{name}</name>
|
||||
<uri>{uri}</uri>
|
||||
<pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
|
||||
</include>
|
7
sdf/mocks/sdf/joint_fixed.sdf
Normal file
7
sdf/mocks/sdf/joint_fixed.sdf
Normal file
|
@ -0,0 +1,7 @@
|
|||
<joint name="{name}" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>{child}::{child}</child>
|
||||
<pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
|
||||
</joint>
|
||||
|
||||
|
36
sdf/mocks/sdf/link.sdf
Normal file
36
sdf/mocks/sdf/link.sdf
Normal file
|
@ -0,0 +1,36 @@
|
|||
<link name="{name}">
|
||||
<pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
|
||||
<inertial>
|
||||
<pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
|
||||
<inertia>
|
||||
<ixx>{ixx}</ixx>
|
||||
<ixy>{ixy}</ixy>
|
||||
<ixz>{ixz}</ixz>
|
||||
<iyy>{iyy}</iyy>
|
||||
<iyz>{iyz}</iyz>
|
||||
<izz>{izz}</izz>
|
||||
</inertia>
|
||||
<mass>{massSDF}</mass>
|
||||
</inertial>
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model:/{stl}</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model:/{stl}</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>{friction}</mu>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</visual>
|
||||
</link>
|
5
sdf/mocks/sdf/model.config
Normal file
5
sdf/mocks/sdf/model.config
Normal file
|
@ -0,0 +1,5 @@
|
|||
|
||||
<?xml version="1.0" ?>
|
||||
<model>
|
||||
<sdf version="1.5">model.sdf</sdf>
|
||||
</model>
|
27
sdf/mocks/sdf/model.sdf
Normal file
27
sdf/mocks/sdf/model.sdf
Normal file
|
@ -0,0 +1,27 @@
|
|||
<?xml version='1.0'?>
|
||||
<sdf version="1.4">
|
||||
<model name="{name}">
|
||||
<link name="{name}">
|
||||
<gravity>0</gravity>
|
||||
<collision name="collision">
|
||||
<mesh>
|
||||
<uri>model:/{stl}</uri>
|
||||
</mesh>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model:/{stl}</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>{friction}</mu>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
71
sdf/mocks/sdf/world.sdf
Normal file
71
sdf/mocks/sdf/world.sdf
Normal file
|
@ -0,0 +1,71 @@
|
|||
<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'/>
|
||||
<scene>
|
||||
<ambient>0.4 0.4 0.4 1</ambient>
|
||||
<background>0.7 0.7 0.7 1</background>
|
||||
<shadows>true</shadows>
|
||||
</scene>
|
||||
<model name='ground_plane'>
|
||||
<static>true</static>
|
||||
<link name='link'>
|
||||
<collision name='collision'>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<bounce/>
|
||||
<contact/>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='visual'>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.8 0.8 1</ambient>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<mass>1</mass>
|
||||
<inertia>
|
||||
<ixx>1</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>1</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>1</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<enable_wind>false</enable_wind>
|
||||
</link>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<self_collide>false</self_collide>
|
||||
</model>
|
||||
|
||||
</world>
|
||||
</sdf>
|
1
sdf/requirements.txt
Normal file
1
sdf/requirements.txt
Normal file
|
@ -0,0 +1 @@
|
|||
argparse
|
161
sdf/src/model/sdf_geometry.py
Normal file
161
sdf/src/model/sdf_geometry.py
Normal file
|
@ -0,0 +1,161 @@
|
|||
import os
|
||||
from helper.fs import FS
|
||||
|
||||
from src.model.sdf_join import SdfJoin
|
||||
import typing
|
||||
import uuid
|
||||
|
||||
|
||||
def from_str(x):
|
||||
assert isinstance(x, str)
|
||||
return x
|
||||
|
||||
|
||||
def from_none(x):
|
||||
assert x is None
|
||||
return x
|
||||
|
||||
|
||||
def from_union(fs, x):
|
||||
for f in fs:
|
||||
try:
|
||||
return f(x)
|
||||
except:
|
||||
pass
|
||||
assert False
|
||||
|
||||
|
||||
def to_class(c, x):
|
||||
assert isinstance(x, c)
|
||||
return x.to_dict()
|
||||
|
||||
|
||||
class SdfGeometryModel:
|
||||
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
|
||||
self.ixy = ixy
|
||||
self.ixz = ixz
|
||||
self.iyy = iyy
|
||||
self.izz = izz
|
||||
self.massSDF = massSDF
|
||||
self.posX = posX
|
||||
self.posY = posY
|
||||
self.posZ = posZ
|
||||
self.eulerX = eulerX
|
||||
self.eulerY = eulerY
|
||||
self.eulerZ = eulerZ
|
||||
self.iyz = iyz
|
||||
self.stl = stl
|
||||
self.link = link
|
||||
self.friction = friction
|
||||
|
||||
@staticmethod
|
||||
def from_dict(obj):
|
||||
assert isinstance(obj, dict)
|
||||
name = from_union([from_str, from_none], obj.get("name"))
|
||||
ixx = from_union([from_str, from_none], obj.get("ixx"))
|
||||
ixy = from_union([from_str, from_none], obj.get("ixy"))
|
||||
ixz = from_union([from_str, from_none], obj.get("ixz"))
|
||||
iyy = from_union([from_str, from_none], obj.get("iyy"))
|
||||
izz = from_union([from_str, from_none], obj.get("izz"))
|
||||
massSDF = from_union([from_str, from_none], obj.get("massSDF"))
|
||||
posX = from_union([from_str, from_none], obj.get("posX"))
|
||||
posY = from_union([from_str, from_none], obj.get("posY"))
|
||||
posZ = from_union([from_str, from_none], obj.get("posZ"))
|
||||
eulerX = from_union([from_str, from_none], obj.get("eulerX"))
|
||||
eulerY = from_union([from_str, from_none], obj.get("eulerY"))
|
||||
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'))
|
||||
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)
|
||||
|
||||
def to_dict(self):
|
||||
result = {}
|
||||
if self.name is not None:
|
||||
result["name"] = from_union([from_str, from_none], self.name)
|
||||
if self.ixx is not None:
|
||||
result["ixx"] = from_union([from_str, from_none], self.ixx)
|
||||
if self.ixy is not None:
|
||||
result["ixy"] = from_union([from_str, from_none], self.ixy)
|
||||
if self.ixz is not None:
|
||||
result["ixz"] = from_union([from_str, from_none], self.ixz)
|
||||
if self.iyy is not None:
|
||||
result["iyy"] = from_union([from_str, from_none], self.iyy)
|
||||
if self.izz is not None:
|
||||
result["izz"] = from_union([from_str, from_none], self.izz)
|
||||
if self.massSDF is not None:
|
||||
result["massSDF"] = from_union([from_str, from_none], self.massSDF)
|
||||
if self.posX is not None:
|
||||
result["posX"] = from_union([from_str, from_none], self.posX)
|
||||
if self.posY is not None:
|
||||
result["posY"] = from_union([from_str, from_none], self.posY)
|
||||
if self.posZ is not None:
|
||||
result["posZ"] = from_union([from_str, from_none], self.posZ)
|
||||
if self.eulerX is not None:
|
||||
result["eulerX"] = from_union([from_str, from_none], self.eulerX)
|
||||
if self.eulerY is not None:
|
||||
result["eulerY"] = from_union([from_str, from_none], self.eulerY)
|
||||
if self.eulerZ is not None:
|
||||
result["eulerZ"] = from_union([from_str, from_none], self.eulerZ)
|
||||
if self.iyz is not None:
|
||||
result["iyz"] = from_union([from_str, from_none], self.iyz)
|
||||
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)
|
||||
if self.friction is not None:
|
||||
result["friction"] = from_union([from_str, from_none], self.eulerZ)
|
||||
return result
|
||||
|
||||
def toJSON(self) -> str:
|
||||
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)
|
||||
|
||||
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)
|
||||
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']):
|
||||
sdf = '\n<model name="assembly">\n'
|
||||
sdf+= ' <link name="base_link">\n'
|
||||
sdf += " <pose>0 0 0 0 0 0</pose>\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'
|
||||
|
||||
sdfJoin += endSDF
|
||||
sdfJoin += '</model>'
|
||||
return sdfJoin
|
||||
|
16
sdf/src/model/sdf_join.py
Normal file
16
sdf/src/model/sdf_join.py
Normal file
|
@ -0,0 +1,16 @@
|
|||
from helper.fs import FS
|
||||
import os
|
||||
|
||||
|
||||
|
||||
class SdfJoin:
|
||||
|
||||
def __init__(self, name, parent, modelAt, child) -> None:
|
||||
self.name = name
|
||||
self.parent = parent
|
||||
self.child = child
|
||||
self.modelAt = modelAt
|
||||
pass
|
||||
|
||||
def toSDF(self):
|
||||
return (FS.readFile(os.path.dirname(os.path.realpath(__file__)) + '/../../mocks/sdf/joint_fixed.sdf')).replace('{name}', self.name,).replace('{parent}', self.parent).replace('{child}', self.child).replace('{posX}', self.modelAt.posX).replace('{posY}', self.modelAt.posY).replace('{posZ}', self.modelAt.posZ).replace('{eulerX}', self.modelAt.eulerX).replace('{eulerY}', self.modelAt.eulerY).replace('{eulerZ}', self.modelAt.eulerZ).replace('{ixx}', self.modelAt.ixx).replace('{ixy}', self.modelAt.ixy).replace('{ixz}', self.modelAt.ixz).replace('{iyy}', self.modelAt.iyy).replace('{iyz}', self.modelAt.iyz).replace('{izz}', self.modelAt.izz)
|
12
sdf/src/usecases/generate_world.py
Normal file
12
sdf/src/usecases/generate_world.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
|
45
sdf/src/usecases/sdf_sub_assembly_usecase.py
Normal file
45
sdf/src/usecases/sdf_sub_assembly_usecase.py
Normal file
|
@ -0,0 +1,45 @@
|
|||
from typing import Optional
|
||||
from src.model.sdf_geometry import SdfGeometryModel
|
||||
|
||||
|
||||
def listGetFirstValue(iterable, default=False, pred=None):
|
||||
return next(filter(pred, iterable), default)
|
||||
|
||||
|
||||
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
|
||||
for key, value in generateSubAssemblyModels.items():
|
||||
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
|
||||
|
||||
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
|
2
setup.py
2
setup.py
|
@ -18,4 +18,4 @@ setuptools.setup(
|
|||
#},
|
||||
#python_requires='>=3.10',
|
||||
packages=setuptools.find_packages(),
|
||||
)
|
||||
)
|
Loading…
Add table
Add a link
Reference in a new issue