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

18
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,
)

11
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'/>

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

18
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__()]
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
class GeometryValidateUseCase:
def call(geometry) -> str:
return

69
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,
)

6
cad_generation/env.json
View file

@ -1,6 +1,4 @@
{
"doc": "/home/idontsudo/framework/asp/out/disk_and_axis_n.FCStd",
"out": "/home/idontsudo/framework/asp/out",
"resultURL": "http://localhost:3002/assembly/save/out",
"projectId": "cubes"
"cadFilePath": "/Users/idontsudo/Desktop/asp-example/disk_and_axis_n.FCStd",
"outPath": "/Users/idontsudo/Desktop/asp-example/"
}

28
cad_generation/helper/file_system_repository.py Normal file
View file

@ -0,0 +1,28 @@
import os
import json
import shutil
class FileSystemRepository:
def readJSON(path: str):
return json.loads((open(path)).read())
def recursiveDeleteFolder(path: str):
shutil.rmtree(path)
pass
def deletingOldAndCreatingNewFolder(path: str):
if FileSystemRepository.isExistsPath(path):
FileSystemRepository.recursiveDeleteFolder(path)
os.makedirs(path)
pass
def isExistsPath(path: str):
return os.path.exists(path)
def writeFile(data, filePath, fileName):
file_to_open = filePath + fileName
f = open(file_to_open, "w", encoding="utf-8", errors="ignore")
f.write(data)
f.close()

15
cad_generation/helper/fs.py
View file

@ -1,15 +0,0 @@
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', encoding='utf-8',
errors='ignore')
f.write(data)
f.close()

26
cad_generation/main.py
View file

@ -1,19 +1,25 @@
import requests
import FreeCAD as App
from helper.fs import FS
from scenarios.robossembler_freecad_export_scenario import RobossemblerFreeCadExportScenario
import shutil
import os
from helper.file_system_repository import FileSystemRepository
from scenarios.robossembler_freecad_export_scenari import (
RobossemblerFreeCadExportScenari,
)
import FreeCADGui as Gui
# obj.Support[0][0].Label
# 'Hex_King'
import FreeCAD as App
def main():
env = FS.readJSON('./env.json')
App.openDocument(env.get('doc'))
RobossemblerFreeCadExportScenario().call(env.get('out'))
# requests.post(url=env.get('resultURL'), files={'zip': open(env.get('out') + '/' + 'generation.zip', "rb"), 'id':env.get('projectId')})
# os.remove('./generation.zip')
env = FileSystemRepository.readJSON("./env.json")
App.openDocument(env.get("cadFilePath"))
RobossemblerFreeCadExportScenari.call(env.get("outPath"))
App.closeDocument(App.ActiveDocument.Name)
freecadQTWindow = Gui.getMainWindow()
freecadQTWindow.close()
main()

54
cad_generation/model/sdf_geometry_model.py
View file

@ -26,7 +26,28 @@ def to_class(c, x):
class SdfGeometryModel:
def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction, centerMassX, centerMassY, centerMassZ,):
def __init__(
self,
name,
ixx,
ixy,
ixz,
iyy,
izz,
massSDF,
posX,
posY,
posZ,
eulerX,
eulerY,
eulerZ,
iyz,
stl,
friction,
centerMassX,
centerMassY,
centerMassZ,
):
self.name = name
self.ixx = ixx
self.ixy = ixy
@ -69,7 +90,27 @@ class SdfGeometryModel:
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 SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction, centerMassX, centerMassY, centerMassZ)
return SdfGeometryModel(
name,
ixx,
ixy,
ixz,
iyy,
izz,
massSDF,
posX,
posY,
posZ,
eulerX,
eulerY,
eulerZ,
iyz,
stl,
friction,
centerMassX,
centerMassY,
centerMassZ,
)
def to_dict(self):
result = {}
@ -105,14 +146,13 @@ class SdfGeometryModel:
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)
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("'", '"')

49
cad_generation/scenarios/robossembler_freecad_export_scenari.py Normal file
View file

@ -0,0 +1,49 @@
from usecases.export_assembly_them_all_usecase import ExportAssemblyThemAllUseCase
import FreeCAD
from usecases.export_usecase import EXPORT_TYPES, ExportUseCase
from usecases.get_sdf_geometry_usecase import SdfGeometryUseCase
from usecases.assembly_parse_usecase import AssemblyParseUseCase
from model.files_generator import FolderGenerator
from helper.file_system_repository import FileSystemRepository
import os
class RobossemblerFreeCadExportScenari:
def call(path):
directory = path
__objs__ = FreeCAD.ActiveDocument.RootObjects
directoryExport = directory + "/"
FileSystemRepository.deletingOldAndCreatingNewFolder(
directoryExport + FolderGenerator.ASSETS.value
)
FileSystemRepository.deletingOldAndCreatingNewFolder(
directoryExport + FolderGenerator.SDF.value,
)
FileSystemRepository.deletingOldAndCreatingNewFolder(
directoryExport
+ FolderGenerator.SDF.value
+ "/"
+ FolderGenerator.MESHES.value
)
FileSystemRepository.deletingOldAndCreatingNewFolder(
directoryExport + FolderGenerator.ASSEMBlY.value
)
f = open(directory + "/step-structure.json", "w")
f.write(AssemblyParseUseCase().toJson())
f.close()
RobossemblerFreeCadExportScenari.geometry(directory)
ExportAssemblyThemAllUseCase().call(directoryExport)
return True
def geometry(outPutsPath: str):
exportUseCase = ExportUseCase.call(outPutsPath, EXPORT_TYPES.OBJ)
for el in SdfGeometryUseCase().call(exportUseCase):
FileSystemRepository.writeFile(
el.toJSON(),
outPutsPath + FolderGenerator.ASSETS.value + "/",
el.name + ".json",
)

52
cad_generation/scenarios/robossembler_freecad_export_scenario.py
View file

@ -1,52 +0,0 @@
from usecases.export_assembly_them_all_usecase import ExportAssemblyThemAllUseCase
import FreeCAD
from usecases.export_usecase import EXPORT_TYPES, 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
import os
import shutil
class RobossemblerFreeCadExportScenario:
def call(self, path):
directory = path + '/' + 'generation'
if os.path.exists(directory):
shutil.rmtree(directory)
if not os.path.exists(directory):
os.makedirs(directory)
__objs__ = FreeCAD.ActiveDocument.RootObjects
directoryExport = directory + '/'
os.makedirs(directoryExport + FolderGenerator.ASSETS.value)
os.makedirs(directoryExport + FolderGenerator.SDF.value)
os.makedirs(directoryExport + FolderGenerator.SDF.value + '/' + FolderGenerator.MESHES.value)
os.makedirs(directoryExport + FolderGenerator.ASSEMBlY.value)
f = open(directory + "/step-structure.json", "w")
f.write(AssemblyParseUseCase().toJson())
f.close()
self.geometry(directory)
ExportAssemblyThemAllUseCase().call(directoryExport)
# shutil.make_archive(directory, 'zip', directory)
# shutil.rmtree(directory)
return True
def geometry(self, outPutsPath: str):
exportUseCase = ExportUseCase.call(outPutsPath,EXPORT_TYPES.OBJ)
for el in SdfGeometryUseCase().call(exportUseCase):
FS.writeFile(el.toJSON(), outPutsPath + '/' + FolderGenerator.ASSETS.value + '/', el.name + '.json',)

104
cad_generation/usecases/export_assembly_them_all_usecase.py
View file

@ -1,11 +1,9 @@
from typing import List
import FreeCAD as App
import Part
from model.join_mesh_model import JoinMeshModel
from model.mesh_part_model import MeshPartModel
from helper.fs import FS
from helper.file_system_repository import FileSystemRepository
from helper.is_solid import is_object_solid
from model.simple_copy_part_model import SimpleCopyPartModel
from model.files_generator import FolderGenerator
@ -15,78 +13,106 @@ import json
class ExportAssemblyThemAllUseCase:
def call(self, path):
assembly = AssemblyParseUseCase().getAsm()
asmStructure = {}
inc = 0
for el in assembly:
if (inc != 0):
asmStructure[inc] = {
"child": el,
"parents": assembly[0:inc]
}
if inc != 0:
asmStructure[inc] = {"child": el, "parents": assembly[0:inc]}
inc += 1
objectsFreeCad = App.ActiveDocument.Objects
asmSolids = {}
for k, v in asmStructure.items():
assemblyParentList = v['parents']
assemblyChild = v['child']
assemblyParentList = v["parents"]
assemblyChild = v["child"]
for el in assemblyParentList:
for solid in objectsFreeCad:
if (el == solid.Label):
if (asmSolids.get(k) is None):
if el == solid.Label:
if asmSolids.get(k) is None:
asmSolids[k] = {
"parents": [],
"child": list(
filter(
lambda x: x.Label == assemblyChild,
objectsFreeCad,
)
)[0],
}
asmSolids[k] = {'parents': [], 'child': list(
filter(lambda x: x.Label == assemblyChild, objectsFreeCad))[0]}
asmSolids[k]['parents'].append(solid)
asmSolids[k]["parents"].append(solid)
inc = 0
for k, v in asmSolids.items():
geometry = {"0": [], "1": []}
if (k != 0):
if k != 0:
App.activeDocument().addObject("Part::Compound", "Compound")
copyLinks = list(
map(lambda el: SimpleCopyPartModel(el), v['parents']))
copyLinks = list(map(lambda el: SimpleCopyPartModel(el), v["parents"]))
if copyLinks != None:
App.activeDocument().Compound.Links = list(
map(lambda el: el.getPart(), copyLinks))
map(lambda el: el.getPart(), copyLinks)
)
object = App.activeDocument().getObject('Compound')
object = App.activeDocument().getObject("Compound")
boundBox = object.Shape.BoundBox
geometry['0'].append(boundBox.XMax)
geometry['0'].append(boundBox.YMax)
geometry['0'].append(boundBox.ZMax)
geometry["0"].append(boundBox.XMax)
geometry["0"].append(boundBox.YMax)
geometry["0"].append(boundBox.ZMax)
os.makedirs(
path + FolderGenerator.ASSEMBlY.value + '/' + '0000' + str(k))
boundBoxChild = v['child'].Shape.BoundBox
geometry['1'].append(boundBoxChild.XMax)
geometry['1'].append(boundBoxChild.YMax)
geometry['1'].append(boundBoxChild.ZMax)
path + FolderGenerator.ASSEMBlY.value + "/" + "0000" + str(k)
)
boundBoxChild = v["child"].Shape.BoundBox
geometry["1"].append(boundBoxChild.XMax)
geometry["1"].append(boundBoxChild.YMax)
geometry["1"].append(boundBoxChild.ZMax)
meshParents = []
for el in v['parents']:
for el in v["parents"]:
meshParents.append(MeshPartModel(el))
joinMesh = JoinMeshModel(meshParents)
for el in meshParents:
el.remove()
import importOBJ
importOBJ.export(joinMesh.mesh, path + FolderGenerator.ASSEMBlY.value +
'/' + '0000' + str(k) + '/' + str(1) + '.obj')
importOBJ.export(
joinMesh.mesh,
path
+ FolderGenerator.ASSEMBlY.value
+ "/"
+ "0000"
+ str(k)
+ "/"
+ str(1)
+ ".obj",
)
joinMesh.remove()
importOBJ.export(v['child'], path + FolderGenerator.ASSEMBlY.value +
'/' + '0000' + str(k) + '/' + str(0) + '.obj')
FS.writeFile(json.dumps(geometry), path + FolderGenerator.ASSEMBlY.value +
'/' + '0000' + str(k) + '/', 'translation.json')
importOBJ.export(
v["child"],
path
+ FolderGenerator.ASSEMBlY.value
+ "/"
+ "0000"
+ str(k)
+ "/"
+ str(0)
+ ".obj",
)
FileSystemRepository.writeFile(
json.dumps(geometry),
path
+ FolderGenerator.ASSEMBlY.value
+ "/"
+ "0000"
+ str(k)
+ "/",
"translation.json",
)
App.ActiveDocument.removeObject("Compound")
for el in copyLinks:
el.remove()
App.activeDocument().recompute()
inc += 1

47
cad_generation/usecases/export_usecase.py
View file

@ -5,32 +5,53 @@ from model.files_generator import FolderGenerator
from helper.is_solid import is_object_solid
from enum import Enum
class EXPORT_TYPES(Enum):
STL = 'STL'
DAO = 'DAO'
OBJ = 'OBJ'
STL = "STL"
DAO = "DAO"
OBJ = "OBJ"
class ExportUseCase:
def call(path: str, type: EXPORT_TYPES):
meshes = {}
for el in App.ActiveDocument.Objects:
if (is_object_solid(el)):
if is_object_solid(el):
match type.value:
case EXPORT_TYPES.STL.value:
Mesh.export([el], path + '/' + FolderGenerator.SDF.value +
'/' + FolderGenerator.MESHES.value + '/' + el.Label + '.stl')
meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
'/' + el.Label + '.stl'
Mesh.export(
[el],
path
+ "/"
+ FolderGenerator.SDF.value
+ "/"
+ FolderGenerator.MESHES.value
+ "/"
+ el.Label
+ ".stl",
)
meshes[el.Label] = (
"/" + FolderGenerator.MESHES.value + "/" + el.Label + ".stl"
)
# case EXPORT_TYPES.DAO.value:
# importDAE.export([el], path + '/' + FolderGenerator.SDF.value +
# '/' + FolderGenerator.MESHES.value + '/' + el.Label + '.dae')
case EXPORT_TYPES.OBJ.value:
import importOBJ
importOBJ.export([el], path + '/' + FolderGenerator.SDF.value +
'/' + FolderGenerator.MESHES.value + '/' + el.Label + '.obj')
meshes[el.Label] = '/' + FolderGenerator.MESHES.value + \
'/' + el.Label + '.obj'
print(300)
importOBJ.export(
[el],
path
+ "/"
+ FolderGenerator.SDF.value
+ "/"
+ FolderGenerator.MESHES.value
+ "/"
+ el.Label
+ ".obj",
)
meshes[el.Label] = (
"/" + FolderGenerator.MESHES.value + "/" + el.Label + ".obj"
)
return meshes

342
cad_stability_check/main.py
View file

@ -3,7 +3,6 @@ from typing import Any, TypeVar, Type, cast
import FreeCAD as App
import json
import importOBJ
import FreeCAD as App
import Draft
import os
import Part
@ -11,21 +10,28 @@ import numpy as np
from typing import TypeAlias
import FreeCADGui as Gui
ISequencesUnion: TypeAlias = dict[str:dict[str:list[str]]]
ISequencesUnion: TypeAlias = dict[str : dict[str : list[str]]]
def importObjAtPath(path: str, inc: int):
importOBJ.insert(u"" + path, App.ActiveDocument.Label)
try:
importOBJ.insert("" + path, App.ActiveDocument.Label)
mesh = App.ActiveDocument.Objects[inc]
shape = Part.Shape()
shape.makeShapeFromMesh(mesh.Mesh.Topology, 0.05)
solid = Part.makeSolid(shape)
Part.show(solid)
App.ActiveDocument.Objects[inc +
1].Label = App.ActiveDocument.Objects[inc].Name
App.ActiveDocument.Objects[inc + 1].Label = App.ActiveDocument.Objects[inc].Name
App.ActiveDocument.removeObject(App.ActiveDocument.Objects[inc].Name)
return App.ActiveDocument.Objects[inc]
except:
print("path")
print(path)
print("inc")
print(inc)
T = TypeVar("T")
@ -52,15 +58,18 @@ def to_class(c: Type[T], x: Any) -> dict:
def euler_to_quaternion(yaw, pitch, roll):
qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - \
np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + \
np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - \
np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + \
np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
roll / 2
) * np.sin(pitch / 2) * np.sin(yaw / 2)
qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
roll / 2
) * np.cos(pitch / 2) * np.sin(yaw / 2)
qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
roll / 2
) * np.sin(pitch / 2) * np.cos(yaw / 2)
qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
roll / 2
) * np.sin(pitch / 2) * np.sin(yaw / 2)
return [qx, qy, qz, qw]
@ -76,7 +85,7 @@ class Coords:
self.z = z
@staticmethod
def from_dict(obj: Any) -> 'Coords':
def from_dict(obj: Any) -> "Coords":
assert isinstance(obj, dict)
x = from_float(obj.get("x"))
y = from_float(obj.get("y"))
@ -96,26 +105,25 @@ class Coords:
class MotionResultModel:
id: str
euler: Coords
quaternion: Coords
position: Coords
def __init__(self, id: str, euler: Coords, position: Coords) -> None:
def __init__(self, id: str, quaternion: Coords, position: Coords) -> None:
self.id = id
self.euler = euler
self.quaternion = quaternion
self.position = position
@staticmethod
def from_dict(obj: Any) -> 'MotionResultModel':
assert isinstance(obj, dict)
def from_dict(obj: Any) -> "MotionResultModel":
id = from_str(obj.get("id"))
euler = Coords.from_dict(obj.get("euler"))
quaternion = Coords.from_dict(obj.get("quaternion"))
position = Coords.from_dict(obj.get("position"))
return MotionResultModel(id, euler, position)
return MotionResultModel(id, quaternion, position)
def to_dict(self) -> dict:
result: dict = {}
result["id"] = from_str(self.id)
result["euler"] = to_class(Coords, self.euler)
result["quaternion"] = to_class(Coords, self.quaternion)
result["position"] = to_class(Coords, self.position)
return result
@ -131,117 +139,144 @@ class SequencesEvaluation:
pass
def assemblyComputed(self):
debug = True
isOk = True
for sequenceNumber, v in self.sequences.items():
for assemblyNumber, assemblySequenced in v.items():
# print(assemblyNumber)
# print()
# if(assemblyNumber == 1 and sequenceNumber == 4 or assemblyNumber == 1 and sequenceNumber == 0):
if(assemblyNumber == 1 and sequenceNumber == 1 and debug):
debug = False
if(sequenceNumber == 0):
sequenceNumber+=1
print(assemblySequenced)
if isOk:
# sequenceNumber += 1
isOk = False
self.comptedAssembly(
assemblySequenced, sequenceNumber, assemblyNumber)
assemblySequenced, sequenceNumber, assemblyNumber
)
pass
def comptedAssembly(self, assembly: list[str], sequenceNumber: int, assemblyNumber: int):
def comptedAssembly(
self, assembly: list[str], sequenceNumber: int, assemblyNumber: int
):
assemblyParts = []
for counter in range(len(assembly)):
importObjAtPath(
self.assemblyDir + 'sdf/meshes/' + assembly[counter] + '.obj',
counter
self.assemblyDir + "generation/meshes/" + assembly[counter] + ".obj",
counter,
)
assemblyParts.append(
{"part": App.ActiveDocument.Objects[counter], "name": assembly[counter]}
)
assemblyParts.append({
"part": App.ActiveDocument.Objects[counter],
"name": assembly[counter]
})
motionResult = json.loads((open(self.assemblyDir + 'stability' + '/' + str(
sequenceNumber + 1) + '/' + str(assemblyNumber) + '/' + 'motion_result.json')).read())
motionResult = json.loads(
(
open(
self.assemblyDir
+ "stability"
+ "/"
+ str(sequenceNumber + 1)
+ "/"
+ str(assemblyNumber)
+ "/"
+ "motion_result.json"
)
).read()
)
simulatorMotionResults: list['MotionResultModel'] = []
simulatorMotionResults: list["MotionResultModel"] = []
for _k, v in motionResult.items():
print(v)
simulatorMotionResults.append(MotionResultModel.from_dict(v))
for el in simulatorMotionResults:
for e in assemblyParts:
# сопоставляем детали
if (el.id == e.get('name')):
if el.id == e.get("name"):
# вычисляем центр детали для перемещения
center = e.get('part').Shape.CenterOfMass
center = e.get("part").Shape.CenterOfMass
# получаем центр деталей из симуляции
new_center = App.Vector(el.position.vector())
# вычисляем вектор смещения
offset = new_center - center
# перемещаем деталь на вектор смещения
e.get('part').Placement.Base += offset
e.get("part").Placement.Base += offset
# # импортируем меш связанный с зоной обьекта
zonePart = importObjAtPath(
self.assemblyDir
+ "stability/zones/meshes/zone_sub_assembly"
+ str(assembly.__len__() - 1)
+ ".obj",
len(App.ActiveDocument.Objects),
)
# # получаем координаты зоны относительно детали за которой закреплена зона
# импортируем меш связанный с зоной обьекта
# zonePart = importObjAtPath(self.assemblyDir + "stability/zones/meshes/zone_sub_assembly" + str(
# assembly.__len__() - 1) + ".obj", len(App.ActiveDocument.Objects))
# получаем координаты зоны относительно детали за которой закреплена зона
print(assemblyNumber)
coords = json.loads(
(open(self.assemblyDir + "stability/zones/sub_assembly_coords_" + str(assemblyNumber - 1) + ".json")).read())
(
open(
self.assemblyDir
+ "stability/zones/sub_assembly_coords_"
+ str(assemblyNumber - 1)
+ ".json"
)
).read()
)
assemblyCounter = len(assemblyParts)
detailWhichZoneBindings = assemblyParts[assemblyCounter - 2].get(
'part')
detailWhichZoneBindings = assemblyParts[assemblyCounter - 2].get("part")
detailStabilityComputed = assemblyParts[assemblyCounter - 1].get(
'part')
relativeCoordinates = coords.get('relativeCoordinates')
detailStabilityComputed = assemblyParts[assemblyCounter - 1].get("part")
relativeCoordinates = coords.get("relativeCoordinates")
relativeEuler = coords.get('relativeEuler')
relativeEuler = coords.get("relativeEuler")
specificEuler = {
'yaw': detailWhichZoneBindings.Placement.Rotation.toEuler()[0] - relativeEuler.get('yaw'),
'pitch': detailWhichZoneBindings.Placement.Rotation.toEuler()[1] - relativeEuler.get('pitch'),
'roll': detailWhichZoneBindings.Placement.Rotation.toEuler()[2] - relativeEuler.get('roll')
"yaw": detailWhichZoneBindings.Placement.Rotation.toEuler()[0]
- relativeEuler.get("yaw"),
"pitch": detailWhichZoneBindings.Placement.Rotation.toEuler()[1]
- relativeEuler.get("pitch"),
"roll": detailWhichZoneBindings.Placement.Rotation.toEuler()[2]
- relativeEuler.get("roll"),
}
quaternion = euler_to_quaternion(specificEuler.get(
'yaw'), specificEuler.get('pitch'), specificEuler.get('roll'))
quaternion = euler_to_quaternion(
specificEuler.get("yaw"),
specificEuler.get("pitch"),
specificEuler.get("roll"),
)
rotation = App.Rotation(
quaternion[0], quaternion[1], quaternion[2], quaternion[3])
quaternion[0], quaternion[1], quaternion[2], quaternion[3]
)
detailStabilityComputed.Placement.Rotation = rotation
centerVector = detailWhichZoneBindings.Shape.CenterOfMass
vector = App.Vector(relativeCoordinates.get(
'x'), relativeCoordinates.get('y'), relativeCoordinates.get('z'))
vector = App.Vector(
relativeCoordinates.get("x"),
relativeCoordinates.get("y"),
relativeCoordinates.get("z"),
)
print(vector)
# TODO
current_center = zonePart.Shape.CenterOfMass
move_vector = App.Vector(centerVector + vector) - current_center
zonePart.Placement.move(move_vector)
# current_center = zonePart.Shape.CenterOfMass
# move_vector = App.Vector(centerVector + vector) - current_center
# zonePart.Placement.move(move_vector)
# computedStabilityResult = computedStability(
# zonePart, detailStabilityComputed)
computedStabilityResult = computedStability(zonePart, detailStabilityComputed)
if sequenceNumber not in self.result.keys():
self.result[sequenceNumber] = []
# self.result[sequenceNumber].append({
# str(assemblyNumber): assembly,
# "result": computedStabilityResult
# })
self.result[sequenceNumber].append(
{str(assemblyNumber): assembly, "result": computedStabilityResult}
)
# for part in App.ActiveDocument.Objects:
# App.ActiveDocument.removeObject(part.Name)
def get_part_center(part):
shape = None
if not hasattr(part, 'Shape'):
if not hasattr(part, "Shape"):
shape = part.Mesh
if hasattr(part, 'Shape'):
if hasattr(part, "Shape"):
shape = part.Shape
center = shape.BoundBox.Center
return App.Vector(center[0],
center[1],
center[2])
return App.Vector(center[0], center[1], center[2])
def move_second_part_to_match_center(first_part, second_part):
@ -255,8 +290,10 @@ def create(part):
clone = Draft.make_clone([part], forcedraft=True)
clone.Scale = App.Vector(1.30, 1.30, 1.30)
clone_corr = (App.Vector(0.4476673941774023, -2.109332894191716, -0.5918687740295264) -
clone.Placement.Base).scale(*App.Vector(-0.25, -0.25, -0.25))
clone_corr = (
App.Vector(0.4476673941774023, -2.109332894191716, -0.5918687740295264)
- clone.Placement.Base
).scale(*App.Vector(-0.25, -0.25, -0.25))
clone.Placement.move(clone_corr)
App.ActiveDocument.recompute()
@ -264,77 +301,94 @@ def create(part):
def getFullPathObj(assemblyFolder: str, name: str):
return assemblyFolder + 'sdf/meshes/' + name + '.obj'
return assemblyFolder + "sdf/meshes/" + name + ".obj"
def computedStability(refElement, childElement):
rootElement = childElement.Shape.BoundBox
# Создание обьекта на котором делается операция пересечения
App.activeDocument().addObject("Part::MultiCommon", "Common")
App.activeDocument().Common.Shapes = [refElement, childElement, ]
App.ActiveDocument.getObject('Common').ViewObject.ShapeColor = getattr(App.ActiveDocument.getObject(
refElement.Name).getLinkedObject(True).ViewObject, 'ShapeColor', App.ActiveDocument.getObject('Common').ViewObject.ShapeColor)
App.ActiveDocument.getObject('Common').ViewObject.DisplayMode = getattr(App.ActiveDocument.getObject(
childElement.Name).getLinkedObject(True).ViewObject, 'DisplayMode', App.ActiveDocument.getObject('Common').ViewObject.DisplayMode)
App.activeDocument().Common.Shapes = [
refElement,
childElement,
]
App.ActiveDocument.getObject("Common").ViewObject.ShapeColor = getattr(
App.ActiveDocument.getObject(refElement.Name).getLinkedObject(True).ViewObject,
"ShapeColor",
App.ActiveDocument.getObject("Common").ViewObject.ShapeColor,
)
App.ActiveDocument.getObject("Common").ViewObject.DisplayMode = getattr(
App.ActiveDocument.getObject(childElement.Name)
.getLinkedObject(True)
.ViewObject,
"DisplayMode",
App.ActiveDocument.getObject("Common").ViewObject.DisplayMode,
)
App.ActiveDocument.recompute()
obj = App.ActiveDocument.getObjectsByLabel('Common')[0]
obj = App.ActiveDocument.getObjectsByLabel("Common")[0]
shp = obj.Shape
bbox = shp.BoundBox
# Если после операции пересечения зона обьекта совпадает с зоной тестируемого обьекта то тест прошел успешно
if bbox.XLength == rootElement.XLength and bbox.YLength == rootElement.YLength and rootElement.ZLength == bbox.ZLength:
if (
bbox.XLength == rootElement.XLength
and bbox.YLength == rootElement.YLength
and rootElement.ZLength == bbox.ZLength
):
return True
return False
def autoStabilityZoneComputed(stepFilesPaths: list[str], directoryStableZonesPath: str):
cadObjects = []
for count in range(len(stepFilesPaths)):
importObjAtPath(stepFilesPaths[count], count)
cadObjects.append(App.ActiveDocument.Objects[count])
assemblesBindings = []
for increment in range(len(cadObjects)):
if (increment != 0):
if increment != 0:
detailForEvaluationZ = cadObjects[increment]
zoneBindingDetailZ = cadObjects[increment-1]
zoneBindingDetailZ = cadObjects[increment - 1]
assemblesBindings.append(
{'zoneBindingDetail': detailForEvaluationZ, 'detailForEvaluation': zoneBindingDetailZ, 'relativeCoordinates': None, 'zonePart': None})
{
"zoneBindingDetail": detailForEvaluationZ,
"detailForEvaluation": zoneBindingDetailZ,
"relativeCoordinates": None,
"zonePart": None,
}
)
for increment in range(len(assemblesBindings)):
el = assemblesBindings[increment]
zoneBindingDetail = el.get('zoneBindingDetail')
zoneBindingDetail = el.get("zoneBindingDetail")
zoneBindingDetailCenterVector = zoneBindingDetail.Shape.CenterOfMass
zoneDetail = create(el.get('detailForEvaluation'))
move_second_part_to_match_center(
el.get('zoneBindingDetail'), zoneDetail)
zoneDetail.Label = 'zone_sub_assembly' + str(increment + 1)
zoneDetail = create(el.get("detailForEvaluation"))
move_second_part_to_match_center(el.get("zoneBindingDetail"), zoneDetail)
zoneDetail.Label = "zone_sub_assembly" + str(increment + 1)
zoneDetail.ViewObject.ShapeColor = (0.40, 0.74, 0.71)
zoneDetail.ViewObject.Transparency = 50
zoneDetailCenterVector = el.get(
'detailForEvaluation').Shape.CenterOfMass
zoneDetailCenterVector = el.get("detailForEvaluation").Shape.CenterOfMass
el['relativeCoordinates'] = {
'x': zoneBindingDetailCenterVector.x - zoneDetailCenterVector.x,
'y': zoneBindingDetailCenterVector.y - zoneDetailCenterVector.y,
'z': zoneBindingDetailCenterVector.z - zoneDetailCenterVector.z
el["relativeCoordinates"] = {
"x": zoneBindingDetailCenterVector.x - zoneDetailCenterVector.x,
"y": zoneBindingDetailCenterVector.y - zoneDetailCenterVector.y,
"z": zoneBindingDetailCenterVector.z - zoneDetailCenterVector.z,
}
el['relativeEuler'] = {
'yaw': zoneBindingDetail.Placement.Rotation.toEuler()[0] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[0],
'pitch': zoneBindingDetail.Placement.Rotation.toEuler()[1] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[1],
'roll': zoneBindingDetail.Placement.Rotation.toEuler()[2] - el.get('detailForEvaluation').Placement.Rotation.toEuler()[2]
el["relativeEuler"] = {
"yaw": zoneBindingDetail.Placement.Rotation.toEuler()[0]
- el.get("detailForEvaluation").Placement.Rotation.toEuler()[0],
"pitch": zoneBindingDetail.Placement.Rotation.toEuler()[1]
- el.get("detailForEvaluation").Placement.Rotation.toEuler()[1],
"roll": zoneBindingDetail.Placement.Rotation.toEuler()[2]
- el.get("detailForEvaluation").Placement.Rotation.toEuler()[2],
}
el['zonePart'] = zoneDetail
el["zonePart"] = zoneDetail
meshesPath = directoryStableZonesPath + 'meshes/'
meshesPath = directoryStableZonesPath + "meshes/"
if not os.path.exists(directoryStableZonesPath):
os.makedirs(directoryStableZonesPath)
if not os.path.exists(meshesPath):
@ -342,23 +396,27 @@ def autoStabilityZoneComputed(stepFilesPaths: list[str], directoryStableZonesPat
zonesSaved = {}
for counter in range(len(assemblesBindings)):
zoneComputed = assemblesBindings[counter]
mesh = zoneComputed.get('zonePart')
zonesSavePath = meshesPath + mesh.Label + '.obj'
mesh = zoneComputed.get("zonePart")
zonesSavePath = meshesPath + mesh.Label + ".obj"
importOBJ.export([mesh], zonesSavePath)
zonesSaved[mesh.Label] = 'meshes/' + mesh.Label + '.obj'
zonesSaved[mesh.Label] = "meshes/" + mesh.Label + ".obj"
for counter in range(len(assemblesBindings)):
el = assemblesBindings[counter]
savePath = zonesSaved[el.get('zonePart').Label]
el['zonePart'] = savePath
el['detailForEvaluation'] = el['detailForEvaluation'].Label
el['zoneBindingDetail'] = el['zoneBindingDetail'].Label
savePath = zonesSaved[el.get("zonePart").Label]
el["zonePart"] = savePath
el["detailForEvaluation"] = el["detailForEvaluation"].Label
el["zoneBindingDetail"] = el["zoneBindingDetail"].Label
json_result = json.dumps(el)
file_to_open = directoryStableZonesPath + \
'sub_assembly_coords_' + str(counter) + '.json'
file_to_open = (
directoryStableZonesPath + "sub_assembly_coords_" + str(counter) + ".json"
)
f = open(file_to_open, 'w', )
f = open(
file_to_open,
"w",
)
f.write(json_result)
f.close()
@ -366,40 +424,38 @@ def autoStabilityZoneComputed(stepFilesPaths: list[str], directoryStableZonesPat
def main():
App.newDocument()
env = json.loads((open('./env.json')).read())
env = json.loads((open("./env.json")).read())
assemblyDir = env.get('aspPath')
sequencesJSON = json.loads((open(assemblyDir + 'sequences.json')).read())
directoryStableZones = assemblyDir + 'stability/zones/'
assemblyDir = env.get("aspPath")
sequencesJSON = json.loads((open(assemblyDir + "sequences.json")).read())
directoryStableZones = assemblyDir + "stability/zones/"
sequences = sequencesJSON.get('sequences')
stepStructure = json.loads(
(open(assemblyDir + 'step-structure.json')).read())
sequences = sequencesJSON.get("sequences")
stepStructure = json.loads((open(assemblyDir + "step-structure.json")).read())
stepFilesPaths = []
for step in stepStructure:
stepFilesPaths.append(assemblyDir+'sdf/meshes/' + step + '.obj')
stepFilesPaths.append(assemblyDir + "generation/meshes/" + step + ".obj")
if not os.path.exists(directoryStableZones):
print('Zones not found automatic calculation started')
print("Zones not found automatic calculation started")
autoStabilityZoneComputed(stepFilesPaths, directoryStableZones)
sequencesJoin = {}
for arrayCounter in range(len(sequences)):
for indexCounter in range(len(sequences[arrayCounter])):
if (indexCounter != 0):
if (sequencesJoin.get(arrayCounter) == None):
if indexCounter != 0:
if sequencesJoin.get(arrayCounter) == None:
sequencesJoin[arrayCounter] = {
indexCounter: sequences[arrayCounter][0:indexCounter+1]
indexCounter: sequences[arrayCounter][0 : indexCounter + 1]
}
else:
sequencesJoin[arrayCounter][indexCounter] = sequences[arrayCounter][0:indexCounter+1]
sequencesJoin[arrayCounter][indexCounter] = sequences[arrayCounter][
0 : indexCounter + 1
]
seqEvaluation = SequencesEvaluation(sequencesJoin, assemblyDir)
seqEvaluation.assemblyComputed()
print(seqEvaluation.result)
main()

2
cg/freecad/Frames/scenarios/robossembler_freecad_export_scenario.py
View file

@ -18,7 +18,7 @@ import shutil
class RobossemblerFreeCadExportScenario:
def call(self):
print(2012)
path = self.qtGuiFeature()
if path == None:
return

1
geometric_feasibility_predicate/README.MD
View file

@ -1 +1,2 @@
/Users/idontsudo/Desktop/FreeCAD.app/Contents/MacOS/FreeCAD
freecadcmd main.py

5
geometric_feasibility_predicate/env.json
View file

@ -1,4 +1,5 @@
{
"cadFilePath":"/home/idontsudo/framework/asp/out/disk_and_axis_n (2).FCStd",
"outPath":"/home/idontsudo/framework/asp/out/"
"cadFilePath": "/Users/idontsudo/Desktop/asp-example/disk_and_axis_n.FCStd",
"outPath": "/Users/idontsudo/Desktop/asp-example/",
"objectIndentation": 0
}

416
geometric_feasibility_predicate/main.py
View file

@ -6,6 +6,16 @@ from typing import List, Dict, Any, TypeVar, Callable, Type, cast
from itertools import repeat
class CoreList(List):
# the list contains only True
def onlyTrue(self) -> bool:
print(self)
for el in self:
if el is not True:
return False
return True
def isInListRange(listIn, index):
try:
listIn[index]
@ -31,8 +41,12 @@ class AllSequences:
inc = 0
for matrix in self.all_sequences:
for index in range(len(matrix)):
result[inc][index] = list(filter(lambda el: el.get(
'number') == matrix[index]+1, self.topologyIds))[0].get('name')
result[inc][index] = list(
filter(
lambda el: el.get("number") == matrix[index] + 1,
self.topologyIds,
)
)[0].get("name")
inc += 1
self.adj_matrix_names = result
pass
@ -59,11 +73,12 @@ class AllSequences:
def findId(self, listMatrix, id):
def filter_odd_num(in_num):
if in_num['name'] == id:
if in_num["name"] == id:
return True
else:
return False
return list(filter(filter_odd_num, listMatrix))[0]['number']
return list(filter(filter_odd_num, listMatrix))[0]["number"]
def iter_paths(self, adj, min_length=6, path=None):
if not path:
@ -94,7 +109,8 @@ class AllSequences:
for k, v in matrix.items():
inc += 1
topologyMatrixNumber[inc] = list(
map(lambda el: self.findId(topologyIds, el), v))
map(lambda el: self.findId(topologyIds, el), v)
)
self.topologyIds = topologyIds
adj = []
matrixSize = matrix.keys().__len__()
@ -102,18 +118,121 @@ class AllSequences:
for k, v in topologyMatrixNumber.items():
adj.append(list(repeat(0, matrixSize)))
for el in v:
adj[inc][el-1] = 1
adj[inc][el - 1] = 1
inc += 1
return self.find_all_sequences(adj)
class VectorModel:
x: float
y: float
z: float
def __init__(self, cadVector) -> None:
self.x = cadVector[0]
self.y = cadVector[1]
self.z = cadVector[2]
pass
def toFreeCadVector(self):
return App.Vector(self.x, self.y, self.z)
class FreeCadRepository:
_solids = []
def getAllSolids(self):
if (self._solids.__len__() == 0):
def isAllObjectsSolids(self) -> List[str]:
result = []
for part in App.ActiveDocument.Objects:
if (self.is_object_solid(part)):
if self.is_object_solid(part) is False:
result.append(part.Label)
return result
def objectSetPosition(self, solid, cadVector):
solid.Placement.Base = cadVector
pass
def objectGetPosition(self, solid) -> VectorModel:
return VectorModel(cadVector=solid.Placement.Base)
def isObjectIntersections(self, part) -> bool:
for solid in self.getAllSolids():
if solid.Label != part.Label:
collisionResult: int = int(part.Shape.distToShape(solid.Shape)[0])
if collisionResult == 0:
return True
return False
def objectHasTouches(self, part, objectIndentation: float) -> List[str]:
positionVector = self.objectGetPosition(part)
result = CoreList()
result.append(self.isObjectIntersections(part=part))
if objectIndentation != 0 and objectIndentation != None:
result.append(
self.axis_movement_and_intersections_observer(
positionVector=positionVector,
alongAxis="x",
objectIndentation=objectIndentation,
part=part,
)
)
result.append(
self.axis_movement_and_intersections_observer(
positionVector=positionVector,
alongAxis="y",
objectIndentation=objectIndentation,
part=part,
)
)
result.append(
self.axis_movement_and_intersections_observer(
positionVector=positionVector,
alongAxis="z",
objectIndentation=objectIndentation,
part=part,
)
)
return result.onlyTrue()
def axis_movement_and_intersections_observer(
self,
positionVector: VectorModel,
alongAxis: str,
objectIndentation: float,
part,
) -> bool:
# UP
positionVector.__setattr__(
alongAxis, positionVector.__getattribute__(alongAxis) + objectIndentation
)
self.objectSetPosition(part, positionVector.toFreeCadVector())
result = self.isObjectIntersections(part=part)
if result:
return True
# DOWN
positionVector.__setattr__(
alongAxis, positionVector.__getattribute__(alongAxis) - objectIndentation
)
positionVector.__setattr__(
alongAxis, positionVector.__getattribute__(alongAxis) - objectIndentation
)
result = self.isObjectIntersections(part=part)
if result:
return True
self.isObjectIntersections(part=part)
# ROLLBACK
positionVector.__setattr__(
alongAxis, positionVector.__getattribute__(alongAxis) + objectIndentation
)
self.objectSetPosition(part, positionVector.toFreeCadVector())
return False
def getAllSolids(self):
if self._solids.__len__() == 0:
for part in App.ActiveDocument.Objects:
if self.is_object_solid(part):
self._solids.append(part)
return self._solids
@ -121,14 +240,12 @@ class FreeCadRepository:
def is_object_solid(self, obj):
if not isinstance(obj, App.DocumentObject):
return False
if hasattr(obj, 'Group'):
if hasattr(obj, "Group"):
return False
if not hasattr(obj, 'Shape'):
if not hasattr(obj, "Shape"):
return False
if not hasattr(obj.Shape, 'Mass'):
return False
if not hasattr(obj.Shape, 'Solids'):
if not hasattr(obj.Shape, "Solids"):
return False
if len(obj.Shape.Solids) == 0:
@ -159,6 +276,7 @@ def to_class(c: Type[T], x: Any) -> dict:
assert isinstance(x, c)
return cast(Any, x).to_dict()
# Вспомогательный класс который делает генрацию JSON на основе пайтон обьектов
@ -168,7 +286,9 @@ class AdjacencyMatrix:
first_detail: str
matrix: Dict[str, List[str]]
def __init__(self, all_parts: List[str], first_detail: str, matrix: Dict[str, List[str]]) -> None:
def __init__(
self, all_parts: List[str], first_detail: str, matrix: Dict[str, List[str]]
) -> None:
self.all_parts = all_parts
self.first_detail = first_detail
self.matrix = matrix
@ -183,11 +303,11 @@ class AdjacencyMatrix:
def validateMatrix(self):
for el in self.all_parts:
if (self.matrix.get(el) == None):
self.matrixError[el] = 'Not found adjacency ' + el
if self.matrix.get(el) == None:
self.matrixError[el] = "Not found adjacency " + el
@staticmethod
def from_dict(obj: Any) -> 'AdjacencyMatrix':
def from_dict(obj: Any) -> "AdjacencyMatrix":
assert isinstance(obj, dict)
all_pars = from_list(from_str, obj.get("allParts"))
first_detail = from_str(obj.get("firstDetail"))
@ -199,12 +319,11 @@ class AdjacencyMatrix:
result: dict = {}
result["allParts"] = from_list(from_str, self.all_parts)
result["firstDetail"] = from_str(self.first_detail)
result["matrix"] = from_dict(
lambda x: from_list(from_str, x), self.matrix)
if (self.matrixError.values().__len__() == 0):
result['matrixError'] = None
result["matrix"] = from_dict(lambda x: from_list(from_str, x), self.matrix)
if self.matrixError.values().__len__() == 0:
result["matrixError"] = None
else:
result['matrixError'] = self.matrixError
result["matrixError"] = self.matrixError
return result
def getDictMatrix(self) -> dict:
@ -225,11 +344,11 @@ def adjacency_matrix_from_dict(s: Any) -> AdjacencyMatrix:
def adjacency_matrix_to_dict(x: AdjacencyMatrix) -> Any:
return to_class(AdjacencyMatrix, x)
# Вспомогательный класс для работы с Freecad
class FreeCadMetaModel(object):
def __init__(self, label, vertex) -> None:
self.label = label
self.vertex = vertex
@ -240,12 +359,18 @@ collision_squares_labels = []
class MeshGeometryCoordinateModel(object):
# Получение геометрии мешей
def __init__(self, x, y, z, label,):
def __init__(
self,
x,
y,
z,
label,
):
self.x = x
self.y = y
self.z = z
self.label = label
self.cadLabel = ''
self.cadLabel = ""
def initializePrimitivesByCoordinate(self, detailSquares):
uuidDoc = str(uuid.uuid1())
@ -259,8 +384,9 @@ class MeshGeometryCoordinateModel(object):
part.Length = 2
part.Placement = App.Placement(
App.Vector(self.x - 1, self.y - 1, self.z - 1),
App.Rotation(App.Vector(0.00, 0.00, 1.00), 0.00))
if (detailSquares.get(self.label) is None):
App.Rotation(App.Vector(0.00, 0.00, 1.00), 0.00),
)
if detailSquares.get(self.label) is None:
detailSquares[self.label] = []
detailSquares[self.label].append(self)
self.cadLabel = uuidDoc
@ -272,19 +398,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', encoding='utf8')
f = open(file_to_open, "w", encoding="utf8")
f.write(data)
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
@ -312,20 +440,20 @@ class GetCollisionAtPrimitiveUseCase(object):
for model in freeCadMetaModels:
activePart = App.ActiveDocument.getObjectsByLabel(model.label)[0]
for key in detailSquares:
if (model.label != key):
if model.label != key:
for renderPrimitive in detailSquares[key]:
primitivePart = App.ActiveDocument.getObjectsByLabel(
renderPrimitive.cadLabel)[0]
renderPrimitive.cadLabel
)[0]
collisionResult: int = int(
activePart.Shape.distToShape(primitivePart.Shape)[0])
if (collisionResult == 0):
activePart.Shape.distToShape(primitivePart.Shape)[0]
)
if collisionResult == 0:
if matrix.get(model.label) == None:
matrix[model.label] = [renderPrimitive.label]
else:
if isUnique(matrix[model.label], renderPrimitive.label):
matrix[model.label].append(
renderPrimitive.label
)
matrix[model.label].append(renderPrimitive.label)
return matrix
@ -354,21 +482,23 @@ class GetPartPrimitiveCoordinatesUseCase(object):
return meshCoordinates
class InitPartsParseUseCase():
class InitPartsParseUseCase:
# Инициализация парсинга
def call(self):
product_details = []
for part in FreeCadRepository().getAllSolids():
if part is not None:
model = FreeCadMetaModel(part.Label, part.Shape.Vertexes)
if (model is not None):
if model is not None:
product_details.append(model)
return product_details
class RenderPrimitiveUseCase(object):
# Рендеринг премитивов
def call(self, meshModels: list[MeshGeometryCoordinateModel], detailSquares) -> None:
def call(
self, meshModels: list[MeshGeometryCoordinateModel], detailSquares
) -> None:
for mesh in meshModels:
mesh.initializePrimitivesByCoordinate(detailSquares)
@ -379,12 +509,12 @@ class ClearWorkSpaceDocumentUseCase(object):
for key in detailSquares:
for renderPrimitive in detailSquares[key]:
primitivePart = App.ActiveDocument.getObjectsByLabel(
renderPrimitive.cadLabel)[0]
renderPrimitive.cadLabel
)[0]
App.ActiveDocument.removeObject(primitivePart.Name)
class RenderPrimitivesScenario(object):
def __init__(
self,
initPartsParseUseCase: InitPartsParseUseCase,
@ -416,7 +546,8 @@ class ClearWorkSpaceDocumentUseCase(object):
for key in detailSquares:
for renderPrimitive in detailSquares[key]:
primitivePart = App.ActiveDocument.getObjectsByLabel(
renderPrimitive.cadLabel)[0]
renderPrimitive.cadLabel
)[0]
App.ActiveDocument.removeObject(primitivePart.Name)
@ -436,6 +567,7 @@ class CadAdjacencyMatrix:
first_detail=GetFirstDetailUseCase().call(),
matrix=matrix,
)
# Матрица основанная на соприкосновении обьектов
def matrixBySurfaces(self):
@ -446,12 +578,17 @@ class CadAdjacencyMatrix:
if part.Label != nextPart.Label:
# Вычисление соприконсоновения площади деталей
collisionResult: int = int(
part.Shape.distToShape(nextPart.Shape)[0])
if (collisionResult == 0):
part.Shape.distToShape(nextPart.Shape)[0]
)
print(collisionResult)
print("collisionResult")
if collisionResult == 0:
matrix[part.Label].append(nextPart.Label)
return AdjacencyMatrix(all_parts=GetAllPartsLabelsUseCase(
).call(), first_detail=GetFirstDetailUseCase().call(),
matrix=matrix
return AdjacencyMatrix(
all_parts=GetAllPartsLabelsUseCase().call(),
first_detail=GetFirstDetailUseCase().call(),
matrix=matrix,
)
@ -472,67 +609,166 @@ def to_ascii_hash(text):
def matrixGetUniqueContact(matrix):
detailsToCheck = []
detailsHashCheck = {}
for k, v in matrix.items():
for el in v:
if (el != k):
if el != k:
hash = to_ascii_hash(k + el)
if (detailsHashCheck.get(hash) == None):
if detailsHashCheck.get(hash) == None:
detailsHashCheck[hash] = hash
detailsToCheck.append({
'child': el,
'parent': k
})
detailsToCheck.append({"child": el, "parent": k})
return detailsToCheck
def intersectionComputed(parts):
class IntersectionComputedUseCase:
def call(parts):
App.activeDocument().addObject("Part::MultiCommon", "Common")
App.activeDocument().Common.Shapes = [parts[0], parts[1]]
App.activeDocument().getObject('Common').ViewObject.ShapeColor = getattr(parts[0].getLinkedObject(True).ViewObject, 'ShapeColor', App.activeDocument().getObject('Common').ViewObject.ShapeColor)
App.activeDocument().getObject('Common').ViewObject.DisplayMode = getattr(parts[0].getLinkedObject(True).ViewObject, 'DisplayMode', App.activeDocument().getObject('Common').ViewObject.DisplayMode)
App.activeDocument().getObject("Common").ViewObject.ShapeColor = getattr(
parts[0].getLinkedObject(True).ViewObject,
"ShapeColor",
App.activeDocument().getObject("Common").ViewObject.ShapeColor,
)
App.activeDocument().getObject("Common").ViewObject.DisplayMode = getattr(
parts[0].getLinkedObject(True).ViewObject,
"DisplayMode",
App.activeDocument().getObject("Common").ViewObject.DisplayMode,
)
App.ActiveDocument.recompute()
area = App.activeDocument().getObject('Common').Shape.Area
App.ActiveDocument.removeObject('Common')
area = App.activeDocument().getObject("Common").Shape.Area
App.ActiveDocument.removeObject("Common")
return area
class ErrorStringModel:
def __init__(self, error: str) -> None:
self.error = error
pass
error: str
def toString(self) -> str:
return json.dumps(
{
"error": self.error,
},
ensure_ascii=False,
indent=4,
)
class IsAllObjectSolidsCheckUseCase:
def call() -> ErrorStringModel:
result = FreeCadRepository().isAllObjectsSolids()
if result.__len__() == 0:
return None
return ErrorStringModel(error="Is not solid objects: " + ",".join(result))
class CheckObjectHasTouchesUseCase:
def call(objectIndentation: float) -> ErrorStringModel:
result = []
for part in FreeCadRepository().getAllSolids():
if (
FreeCadRepository().objectHasTouches(
part=part, objectIndentation=objectIndentation
)
is False
):
result.append(part.Label)
if result.__len__() == 0:
return None
return ErrorStringModel(
error="Solids bodies have no recounts: " + ",".join(result)
)
class CheckCadIntersectionObjects:
report = []
def call() -> bool:
FreeCadRepository().getAllSolids()
return False
class ExitFreeCadUseCase:
def call():
import FreeCADGui as Gui
freecadQTWindow = Gui.getMainWindow()
freecadQTWindow.close()
# class CheckValidIntersectionUseCase:
# def call() -> ErrorStringModel:
# for part in FreeCadRepository().getAllSolids():
# print(part)
# FreeCadRepository().obj
# pass
def main():
env = FS.readJSON('env.json')
cadFile = env['cadFilePath']
outPath = env['outPath']
if (cadFile == None):
return TypeError('CadFile not found env.json')
App.open(u'' + cadFile)
env = FS.readJSON("env.json")
cadFilePath = str(env["cadFilePath"])
outPath = str(env["outPath"])
objectIndentation = float(env["objectIndentation"])
if cadFilePath == None:
return TypeError("CadFile not found env.json")
App.open("" + cadFilePath)
# isAllObjectSolidsCheckUseCase = IsAllObjectSolidsCheckUseCase.call()
# if isAllObjectSolidsCheckUseCase != None:
# FS.writeFile(isAllObjectSolidsCheckUseCase.toString(), outPath, 'error.json')
# ExitFreeCadUseCase.call()
# return
# checkObjectHasTouchesUseCase = CheckObjectHasTouchesUseCase.call(objectIndentation)
# if checkObjectHasTouchesUseCase != None:
# FS.writeFile(checkObjectHasTouchesUseCase.toString(), outPath, 'error.json')
# ExitFreeCadUseCase.call()
# return
topologyMatrix = CadAdjacencyMatrix().matrixBySurfaces()
import json
sequences = json.dumps({"sequences": AllSequences(
topologyMatrix.matrix).adj_matrix_names}, ensure_ascii=False, indent=4)
sequences = json.dumps(
{"sequences": AllSequences(topologyMatrix.matrix).adj_matrix_names},
ensure_ascii=False,
indent=4,
)
matrix = topologyMatrix.matrix
contacts = matrixGetUniqueContact(matrix)
intersection_geometry = {
'status':True,
'recalculations':None
}
intersection_geometry = {"status": True, "recalculations": None}
for el in contacts:
child = App.ActiveDocument.getObjectsByLabel(el.get('child'))[0]
parent = App.ActiveDocument.getObjectsByLabel(el.get('parent'))[0]
area = intersectionComputed([child,parent])
if(area != 0.0):
if(intersection_geometry.get('recalculations') == None):
intersection_geometry['status'] = False
intersection_geometry['recalculations'] = []
intersection_geometry['recalculations'].append({
'area':area,
'connect': el.get('child') + ' ' + el.get('parent')
})
child = App.ActiveDocument.getObjectsByLabel(el.get("child"))[0]
parent = App.ActiveDocument.getObjectsByLabel(el.get("parent"))[0]
area = IntersectionComputedUseCase.call([child, parent])
if area != 0.0:
if intersection_geometry.get("recalculations") == None:
intersection_geometry["status"] = False
intersection_geometry["recalculations"] = []
intersection_geometry["recalculations"].append(
{"area": area, "connect": el.get("child") + " " + el.get("parent")}
)
FS.writeFile(
json.dumps(intersection_geometry, ensure_ascii=False, indent=4),
outPath,
"intersection_geometry.json",
)
FS.writeFile(sequences, outPath, "sequences.json")
FS.writeFile(
json.dumps(topologyMatrix.to_dict(), ensure_ascii=False, indent=4),
outPath,
"adjacency_matrix.json",
)
ExitFreeCadUseCase.call()
FS.writeFile(json.dumps(intersection_geometry, ensure_ascii=False, indent=4), outPath, 'intersection_geometry.json')
FS.writeFile(sequences, outPath, 'sequences.json')
FS.writeFile(json.dumps(topologyMatrix.to_dict(),
ensure_ascii=False, indent=4), outPath, 'adjacency_matrix.json')
main()

116
insertion_vector_predicate/.gitignore vendored Normal file
View file

@ -0,0 +1,116 @@
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class
# C extensions
*.so
# Distribution / packaging
.Python
env/
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
wheels/
*.egg-info/
.installed.cfg
*.egg
# PyInstaller
# Usually these files are written by a python script from a template
# before PyInstaller builds the exe, so as to inject date/other infos into it.
*.manifest
*.spec
# Installer logs
pip-log.txt
pip-delete-this-directory.txt
# Unit test / coverage reports
htmlcov/
.tox/
.coverage
.coverage.*
.cache
nosetests.xml
coverage.xml
*.cover
.hypothesis/
# Translations
*.mo
*.pot
# Django stuff:
*.log
local_settings.py
# Flask stuff:
instance/
.webassets-cache
# Scrapy stuff:
.scrapy
# Sphinx documentation
docs/_build/
# PyBuilder
target/
# Jupyter Notebook
.ipynb_checkpoints
# pyenv
.python-version
# celery beat schedule file
celerybeat-schedule
# SageMath parsed files
*.sage.py
# dotenv
.env
# virtualenv
.venv
venv/
ENV/
# Spyder project settings
.spyderproject
.spyproject
# Rope project settings
.ropeproject
# mkdocs documentation
/site
# mypy
.mypy_cache/
# blender backup files
*.blend1
install_plugin_cad.sh
.vscode
.DS_Store
# emacs backup files
~*
*~
*#
.#*
\#*\#
out/
/env.json

19
insertion_vector_predicate/README.md Normal file
View file

@ -0,0 +1,19 @@
# Start dev
create env.json
```json
{
"cadDoc":"CAD_DOC_PATH_REPLACE",
"sequences":"SEQUENCES_PATH_REPLACE",
"aspDir":"ASP_DIR_REPLACE"
}
```
# Command generation assets
freecad generate.py
# Command generation insertion vectors
git submodule update --init
conda env create -f assembly/environment.yml
conda activate assembly
python3 main.py

205
insertion_vector_predicate/generate.py Normal file
View file

@ -0,0 +1,205 @@
from typing import List
import FreeCAD as App
import Part
import Mesh
import Part
import MeshPart
import os
import json
import FreeCADGui as Gui
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', encoding='utf-8',
errors='ignore')
f.write(data)
f.close()
def createFolder(path: str):
if (not os.path.exists(path)):
return os.mkdir(path)
class SimpleCopyPartModel:
id = None
copyLink = None
label = None
part = None
def getPart(self):
return self.part
def __init__(self, part) -> None:
try:
from random import randrange
self.id = str(randrange(1000000))
childObj = part
print(part)
__shape = Part.getShape(
childObj, '', needSubElement=False, refine=False)
obj = App.ActiveDocument.addObject('Part::Feature', self.id)
obj.Shape = __shape
self.part = obj
self.label = obj.Label
App.ActiveDocument.recompute()
except Exception as e:
print(e)
def remove(self):
App.ActiveDocument.removeObject(self.label)
class MeshPartModel:
id = None
mesh = None
def __init__(self, part) -> None:
try:
from random import randrange
self.id = 'mesh' + str(randrange(1000000))
document = App.ActiveDocument
mesh = document.addObject("Mesh::Feature", self.id)
shape = Part.getShape(part, "")
mesh.Mesh = MeshPart.meshFromShape(
Shape=shape, LinearDeflection=20, AngularDeflection=0.1, Relative=False)
mesh.Label = self.id
self.mesh = mesh
except Exception as e:
print(e)
pass
def remove(self):
try:
App.ActiveDocument.removeObject(self.mesh.Label)
except Exception as e:
print(e)
class JoinMeshModel:
id = None
mesh = None
def __init__(self, meshesPartModels: list['MeshPartModel']) -> None:
meshes = []
from random import randrange
for el in meshesPartModels:
meshes.append(el.mesh.Mesh)
self.id = 'MergedMesh' + str(randrange(1000000))
doc = App.ActiveDocument
merged_mesh = Mesh.Mesh()
for el in meshes:
merged_mesh.addMesh(el)
new_obj = doc.addObject("Mesh::Feature", self.id)
new_obj.Mesh = merged_mesh
new_obj.ViewObject.DisplayMode = "Flat Lines"
self.mesh = new_obj
def remove(self):
try:
App.ActiveDocument.removeObject(self.id)
except Exception as e:
print(e)
class ExportAssemblyThemAllUseCase:
def call(self, path:str, assemblys:list[str]):
assembly = assemblys
asmStructure = {}
inc = 0
for el in assembly:
if (inc != 0):
asmStructure[inc] = {
"child": el,
"parents": assembly[0:inc]
}
inc += 1
objectsFreeCad = App.ActiveDocument.Objects
asmSolids = {}
for k, v in asmStructure.items():
assemblyParentList = v['parents']
assemblyChild = v['child']
for el in assemblyParentList:
for solid in objectsFreeCad:
if (el == solid.Label):
if (asmSolids.get(k) is None):
asmSolids[k] = {'parents': [], 'child': list(
filter(lambda x: x.Label == assemblyChild, objectsFreeCad))[0]}
asmSolids[k]['parents'].append(solid)
inc = 0
for k, v in asmSolids.items():
geometry = {"0": [], "1": []}
if (k != 0):
App.activeDocument().addObject("Part::Compound", "Compound")
copyLinks = list(
map(lambda el: SimpleCopyPartModel(el), v['parents']))
if copyLinks != None:
App.activeDocument().Compound.Links = list(
map(lambda el: el.getPart(), copyLinks))
object = App.activeDocument().getObject('Compound')
boundBox = object.Shape.BoundBox
geometry['0'].append(boundBox.XMax)
geometry['0'].append(boundBox.YMax)
geometry['0'].append(boundBox.ZMax)
boundBoxChild = v['child'].Shape.BoundBox
geometry['1'].append(boundBoxChild.XMax)
geometry['1'].append(boundBoxChild.YMax)
geometry['1'].append(boundBoxChild.ZMax)
meshParents = []
for el in v['parents']:
meshParents.append(MeshPartModel(el))
joinMesh = JoinMeshModel(meshParents)
for el in meshParents:
el.remove()
import importOBJ
importOBJ.export(joinMesh.mesh, path + str(1) + '.obj')
joinMesh.remove()
importOBJ.export(v['child'], path + str(0) + '.obj')
FS.writeFile(json.dumps(geometry), path, 'translation.json')
App.ActiveDocument.removeObject("Compound")
for el in copyLinks:
el.remove()
App.activeDocument().recompute()
inc += 1
def main():
env = FS.readJSON('./env.json')
env.get('cadDoc')
aspDir = env.get('aspDir')
sequences = FS.readJSON(env.get('sequences')).get('sequences')
App.openDocument(env.get('cadDoc'))
for sequencyNumber in range(len(sequences)):
FS.createFolder(aspDir + 'assemblys/')
mainFolder = aspDir + 'assemblys/' + str(sequencyNumber) + '/'
FS.createFolder(mainFolder)
for subSequenceNumber in range(len(sequences[sequencyNumber])):
if(subSequenceNumber != 0):
subFolder = aspDir + 'assemblys/' + \
str(sequencyNumber) + '/' + str(subSequenceNumber) + '/'
FS.createFolder(subFolder)
ExportAssemblyThemAllUseCase().call(path=subFolder,assemblys=sequences[sequencyNumber][0:subSequenceNumber+1])
App.closeDocument(App.ActiveDocument.Name)
freecadQTWindow = Gui.getMainWindow()
freecadQTWindow.close()
main()

107
insertion_vector_predicate/main.py
View file

@ -1,4 +1,14 @@
# Алгоритм генерации графа с помощью вычисления векторов вставки при разборке изделия
import os
import sys
project_base_dir = os.path.abspath(os.path.join(
os.path.dirname(os.path.abspath(__file__)), './')) + '/assembly/'
sys.path.append(project_base_dir)
sys.path.append(project_base_dir + '/baselines/')
sys.path.append(project_base_dir + '/assets/')
from scipy.spatial.transform import Rotation
import shutil
from spatialmath import *
@ -9,17 +19,31 @@ from assembly.baselines.run_joint_plan import PyPlanner
from assembly.assets.subdivide import subdivide_to_size
import numpy as np
import json
import sys
import os
import trimesh
import re
def merge_meshes(meshes):
# Создание пустого меша
merged_mesh = trimesh.Trimesh()
# Объединение каждого меша в один
for mesh in meshes:
merged_mesh = trimesh.util.concatenate(
[merged_mesh, trimesh.load(mesh)])
i = True
while i:
if merged_mesh.fill_holes():
i = False
return merged_mesh
os.environ['OMP_NUM_THREADS'] = '1'
project_base_dir = os.path.abspath(os.path.join(
os.path.dirname(os.path.abspath(__file__)), './')) + '/assembly/'
sys.path.append(project_base_dir)
sys.path.append(project_base_dir + '/baselines/')
sys.path.append(project_base_dir + '/assets/')
class FS:
@ -43,6 +67,10 @@ class FS:
def readFolder(pathFolder: str):
return list(map(lambda el: pathFolder + '/' + el, os.listdir(pathFolder)))
def createFolder(path: str):
if (not os.path.exists(path)):
return os.mkdir(path)
def listGetFirstValue(iterable, default=False, pred=None):
return next(filter(pred, iterable), default)
@ -56,31 +84,44 @@ def filterModels(filterModels, filterModelsDescription):
return models
def main():
from argparse import ArgumentParser
parser = ArgumentParser()
parser.add_argument('--asp-path', type=str, required=True)
args = parser.parse_args()
aspDir = args.asp_dir
# mesh1 = trimesh.load('/Users/idontsudo/framework/asp/out/sdf-generation/meshes/Cube.obj')
# mesh2 = trimesh.load('/Users/idontsudo/framework/asp/out/sdf-generation/meshes/Cube001.obj')
# # Объединение мешей
# merged_mesh = merge_meshes([mesh1, mesh2])
# # Сохранение объединенного меша в файл
# merged_mesh.export('merged.obj')
def main():
# from argparse import ArgumentParser
# parser = ArgumentParser()
# parser.add_argument('--asp-path', type=str, required=True)
# args = parser.parse_args()
# aspDir = args.asp_dir
# # Коректировка пути до папки с генерацией ASP
# if (aspDir == None):
# args.print_helper()
# if (aspDir[aspDir.__len__() - 1] != '/'):
# aspDir += '/'
aspDir = '/home/idontsudo/framework/asp/out/'
sequences = FS.readJSON(aspDir + 'sequences.json').get('sequences')
# Коректировка пути до папки с генерацией ASP
if (aspDir == None):
args.print_helper()
if (aspDir[aspDir.__len__() - 1] != '/'):
aspDir += '/'
# Получение списка папок с .obj обьектами
assembles = FS.readFolder(aspDir + 'assembly')
assemblyDirNormalize = []
for el in assembles:
for el in FS.readFolder(aspDir + 'assemblys'):
for e in FS.readFolder(el):
try:
# Пост обработка .obj обьектов
process_mesh(source_dir=el, target_dir=el +
'/process/', subdivide=el, verbose=True)
assemblyDirNormalize.append(el + '/process/')
process_mesh(source_dir=e, target_dir=e +
'/process/', subdivide=e, verbose=True)
assemblyDirNormalize.append(e + '/process/')
except Exception as e:
print('ERRROR:')
print(e)
print(assemblyDirNormalize)
for el in assemblyDirNormalize:
asset_folder = os.path.join(project_base_dir, aspDir)
assembly_dir = os.path.join(asset_folder, el)
@ -113,13 +154,13 @@ def main():
try:
planner = PyPlanner(assembly_dir, 'process', still_ids=[1],)
status, t_plan, path = planner.plan(
planner_name=args.planner,
step_size=args.step_size,
max_time=args.max_time,
seed=args.seed,
planner_name='rrt',
step_size=None,
max_time=None,
seed=1,
return_path=True,
simplify=args.simplify,
render=args.render
simplify=False,
render=False
)
print(f'Status: {status}, planning time: {t_plan}')

1
insertion_vector_predicate/requirements.txt
View file

@ -1,2 +1,3 @@
spatialmath
scipy
uuid

30
pddl/main.py
View file

@ -1,26 +1,30 @@
import argparse
from helper.fs import FS
from src.model.asm4_structure import Asm4Structure
from src.model.robossembler_assets import RobossemblerAssets
from src.usecases.assembly_to_pddl_use_case import AssemblyToPddlUseCase
# python3 main.py --stepStructurePath --outPath /Users/idontsudo/robo/pddl/out/
# python3 main.py --stepStructurePath /Users/idontsudo/framework/asp/out/step-structure.json --outPath /Users/idontsudo/robo/pddl/out/
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--stepStructurePath', help='json step by FreeCad')
parser.add_argument('--outPath', help='save pddl path')
parser.add_argument("--sequencesPath", help="sequences path")
parser.add_argument("--outPath", help="save pddl path")
parser.add_argument("--robossemblerDbPath", help="robossembler_db is require")
args = parser.parse_args()
if args.stepStructurePath == None or args.outPath == None:
if (
args.sequencesPath == None
or args.outPath == None
or args.robossemblerDbPath == None
):
parser.print_help()
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')
data = FS.readJSON(args.sequencesPath)
robossemblerDb = FS.readJSON(args.robossemblerDbPath)
assemblyToPddlUseCase = AssemblyToPddlUseCase.call(
assembly=data, robossemblerDb=robossemblerDb
)
FS.writeFile(assemblyToPddlUseCase["problem"], args.outPath, "problem.pddl")
FS.writeFile(assemblyToPddlUseCase["domain"], args.outPath, "domain.pddl")

28
pddl/mocks/domain.txt
View file

@ -1,9 +1,7 @@
;; Modified domain taken from
;; "Knowledge transfer in robot manipulation tasks" by Jacob O. Huckaby 2014
(define (domain robossembler)
(:requirements :strips :typing :adl :fluents :durative-actions)
(:types
printer workspace - zone
workspace - zone
part
arm
assembly
@ -12,9 +10,29 @@
(:predicates
(arm_available ?a - arm)
(part_at ?p - part ?z - zone)
(printer_ready ?p - printer)
(part_of ?part - part ?whole - assembly)
(assembly_order ?prev ?next - assembly)
(assembled ?whole - assembly ?z - zone)
)
);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;
(:functions)
(:durative-action assemble
:parameters (?p - part ?prev ?next - assembly ?w - workspace ?arm - arm)
:duration (= ?duration 5)
:condition (and
(at start (assembled ?prev ?w))
(at start (part_at ?p ?w))
(at start (part_of ?p ?next))
(at start (arm_available ?arm))
(at start (assembly_order ?prev ?next))
)
:effect (and
(at start (not (arm_available ?arm)))
(at end (not (part_at ?p ?w)))
(at end (arm_available ?arm))
(at end (assembled ?next ?w))
)
)
)

25
pddl/mocks/problem.txt Normal file
View file

@ -0,0 +1,25 @@
(define (problem robossembler-p1)
(:domain robossembler)
(:objects
;; information from Scene
rasmt - arm
workspace1 - workspace
;; information from CAD
${types}
)
(:init
;; information from Scene
(assembled subasm0 workspace1)
${part_at}
(arm_available rasmt)
;; information from CAD
${assembled}
)
(:goal
(and
;; information from CAD
(assembled ${target} workspace1)
)
)
)-=

399
pddl/src/model/robossembler_assets.py Normal file
View file

@ -0,0 +1,399 @@
from dataclasses import dataclass
import os
from returns.result import Result, Success, Failure
from typing import Optional, Any, List, TypeVar, Callable, Type, cast
from enum import Enum
from helper.fs import FS
T = TypeVar("T")
EnumT = TypeVar("EnumT", bound=Enum)
def from_float(x: Any) -> float:
assert isinstance(x, (float, int)) and not isinstance(x, bool)
return float(x)
def from_none(x: Any) -> Any:
return x
def from_union(fs, x):
for f in fs:
try:
return f(x)
except:
pass
assert False
def to_float(x: Any) -> float:
assert isinstance(x, float)
return x
def from_str(x: Any) -> str:
assert isinstance(x, str)
return x
def from_int(x: Any) -> int:
assert isinstance(x, int) and not isinstance(x, bool)
return x
def from_list(f: Callable[[Any], T], x: Any) -> List[T]:
assert isinstance(x, list)
return [f(y) for y in x]
def to_class(c: Type[T], x: Any) -> dict:
assert isinstance(x, c)
return cast(Any, x).to_dict()
def to_enum(c: Type[EnumT], x: Any) -> EnumT:
assert isinstance(x, c)
return x.value
@dataclass
class Model:
name: Optional[str] = None
id: Optional[str] = None
path: Optional[str] = None
@staticmethod
def from_dict(obj: Any) -> "Model":
assert isinstance(obj, dict)
name = from_union([from_str, from_none], obj.get("name"))
id = from_union([from_str, from_none], obj.get("id"))
path = from_union([from_str, from_none], obj.get("path"))
return Model(name, id, path)
def to_dict(self) -> dict:
result: dict = {}
if self.name is not None:
result["name"] = from_union([from_str, from_none], self.name)
if self.id is not None:
result["id"] = from_union([from_str, from_none], self.id)
if self.path is not None:
result["path"] = from_union([from_str, from_none], self.path)
return result
@dataclass
class Pose:
x: Optional[float] = None
y: Optional[float] = None
z: Optional[float] = None
roll: Optional[float] = None
pitch: Optional[float] = None
yaw: Optional[float] = None
@staticmethod
def from_dict(obj: Any) -> "Pose":
assert isinstance(obj, dict)
x = from_union([from_float, from_none], obj.get("x"))
y = from_union([from_float, from_none], obj.get("y"))
z = from_union([from_float, from_none], obj.get("z"))
roll = from_union([from_float, from_none], obj.get("roll"))
pitch = from_union([from_float, from_none], obj.get("pitch"))
yaw = from_union([from_float, from_none], obj.get("yaw"))
return Pose(x, y, z, roll, pitch, yaw)
def to_dict(self) -> dict:
result: dict = {}
if self.x is not None:
result["x"] = from_union([to_float, from_none], self.x)
if self.y is not None:
result["y"] = from_union([to_float, from_none], self.y)
if self.z is not None:
result["z"] = from_union([to_float, from_none], self.z)
if self.roll is not None:
result["roll"] = from_union([to_float, from_none], self.roll)
if self.pitch is not None:
result["pitch"] = from_union([to_float, from_none], self.pitch)
if self.yaw is not None:
result["yaw"] = from_union([to_float, from_none], self.yaw)
return result
class TypeEnum(Enum):
ASSET = "asset"
LIGHT = "light"
ROBOT = "robot"
@dataclass
class Instance:
model_name: Optional[str] = None
model_id: Optional[str] = None
id: Optional[str] = None
pose: Optional[Pose] = None
scale: Optional[int] = None
type: Optional[TypeEnum] = None
parent: Optional[str] = None
light_type: Optional[str] = None
intencity: Optional[int] = None
diffuse: Optional[List[float]] = None
spot_angle: Optional[int] = None
@staticmethod
def from_dict(obj: Any) -> "Instance":
assert isinstance(obj, dict)
model_name = from_union([from_str, from_none], obj.get("model_name"))
model_id = from_union([from_str, from_none], obj.get("model_id"))
id = from_union([from_str, from_none], obj.get("id"))
pose = from_union([Pose.from_dict, from_none], obj.get("pose"))
scale = from_union([from_int, from_none], obj.get("scale"))
type = from_union([TypeEnum, from_none], obj.get("type"))
parent = from_union([from_str, from_none], obj.get("parent"))
light_type = from_union([from_str, from_none], obj.get("light_type"))
intencity = from_union([from_int, from_none], obj.get("intencity"))
diffuse = from_union(
[lambda x: from_list(from_float, x), from_none], obj.get("diffuse")
)
spot_angle = from_union([from_int, from_none], obj.get("spot_angle"))
return Instance(
model_name,
model_id,
id,
pose,
scale,
type,
parent,
light_type,
intencity,
diffuse,
spot_angle,
)
def fromMappingInstanceAtModel(
self, models: List[Model]
) -> "MappingInstanceAtModel":
for el in models:
if el.id == self.model_id:
return MappingInstanceAtModel(instance=self, model=el)
return Failure("not found model at {self.model_id} ")
def to_dict(self) -> dict:
result: dict = {}
if self.model_name is not None:
result["model_name"] = from_union([from_str, from_none], self.model_name)
if self.model_id is not None:
result["model_id"] = from_union([from_str, from_none], self.model_id)
if self.id is not None:
result["id"] = from_union([from_str, from_none], self.id)
if self.pose is not None:
result["pose"] = from_union(
[lambda x: to_class(Pose, x), from_none], self.pose
)
if self.scale is not None:
result["scale"] = from_union([from_int, from_none], self.scale)
if self.type is not None:
result["type"] = from_union(
[lambda x: to_enum(TypeEnum, x), from_none], self.type
)
if self.parent is not None:
result["parent"] = from_union([from_str, from_none], self.parent)
if self.light_type is not None:
result["light_type"] = from_union([from_str, from_none], self.light_type)
if self.intencity is not None:
result["intencity"] = from_union([from_int, from_none], self.intencity)
if self.diffuse is not None:
result["diffuse"] = from_union(
[lambda x: from_list(to_float, x), from_none], self.diffuse
)
if self.spot_angle is not None:
result["spot_angle"] = from_union([from_int, from_none], self.spot_angle)
return result
class BasePose:
def __init__(self, x: float, y: float, z: float, **kwargs):
self.x = x
self.y = y
self.z = z
def toPose(self, sdfXmlMock: str):
return (
sdfXmlMock.replace("{x}", str(self.x))
.replace("{y}", str(self.y))
.replace("{z}", str(self.z))
)
class MappingInstanceAtModel(BasePose):
instance: Instance
model: Model
def __init__(self, instance: Instance, model: Model) -> None:
self.instance = instance
self.model = model
pass
def toSDF(self):
pose = self.instance.pose
match self.instance.type:
case TypeEnum.ASSET:
mock = FS.readFile(
os.path.dirname(os.path.realpath(__file__))
+ "/../mocks/model_include_sdf.xml"
)
# mockPose = self.toPose(mock)
return (
mock.replace("{name}", str(self.model.name))
.replace("{x}", str(pose.x))
.replace("{y}", str(pose.y))
.replace("{z}", str(pose.z))
.replace("{pitch}", str(pose.pitch))
.replace("{yaw}", str(pose.yaw))
.replace("{roll}", str(pose.roll))
.replace("{uri}", str(self.model.path))
)
case TypeEnum.LIGHT:
pathMock = (
os.path.dirname(os.path.realpath(__file__))
+ "/../mocks/light_sdf.xml"
)
return (
FS.readFile(pathMock)
.replace("{x}", str(pose.x))
.replace("{y}", str(pose.y))
.replace("{z}", str(pose.z))
.replace("{pitch}", str(pose.pitch))
.replace("{yaw}", str(pose.yaw))
.replace("{roll}", str(pose.roll))
.replace("{type_light}", str(self.instance.light_type))
.replace("{name_light}", str("132"))
.replace("{r}", self.instance.diffuse[0])
.replace("{g}", self.instance.diffuse[1])
.replace("{b}", self.instance.diffuse[2])
.replace("{a}", self.instance.diffuse[3])
)
@dataclass
class Gravity:
x: Optional[int] = None
y: Optional[int] = None
z: Optional[float] = None
@staticmethod
def from_dict(obj: Any) -> "Gravity":
assert isinstance(obj, dict)
x = from_union([from_int, from_none], obj.get("x"))
y = from_union([from_int, from_none], obj.get("y"))
z = from_union([from_float, from_none], obj.get("z"))
return Gravity(x, y, z)
def to_dict(self) -> dict:
result: dict = {}
if self.x is not None:
result["x"] = from_union([from_int, from_none], self.x)
if self.y is not None:
result["y"] = from_union([from_int, from_none], self.y)
if self.z is not None:
result["z"] = from_union([to_float, from_none], self.z)
return result
@dataclass
class Physics:
engine_name: Optional[str] = None
gravity: Optional[Gravity] = None
@staticmethod
def from_dict(obj: Any) -> "Physics":
assert isinstance(obj, dict)
engine_name = from_union([from_str, from_none], obj.get("engine_name"))
gravity = from_union([Gravity.from_dict, from_none], obj.get("gravity"))
return Physics(engine_name, gravity)
def to_dict(self) -> dict:
result: dict = {}
if self.engine_name is not None:
result["engine_name"] = from_union([from_str, from_none], self.engine_name)
if self.gravity is not None:
result["gravity"] = from_union(
[lambda x: to_class(Gravity, x), from_none], self.gravity
)
return result
def toSDF(self) -> str:
pathMock = os.path.dirname(os.path.realpath(__file__)) + "/../mocks/world.xml"
gravity = self.gravity
return (
FS.readFile(pathMock)
.replace("{gravity_x}", str(gravity.x))
.replace("{gravity_y}", str(gravity.y))
.replace("{gravity_z}", str(gravity.z))
.replace("{engine_type}", str(self.engine_name))
)
@dataclass
class RobossemblerAssets:
models: Optional[List[Model]] = None
instances: Optional[List[Instance]] = None
physics: Optional[Physics] = None
@staticmethod
def from_dict(obj: Any) -> "RobossemblerAssets":
assert isinstance(obj, dict)
models = from_union(
[lambda x: from_list(Model.from_dict, x), from_none], obj.get("models")
)
instances = from_union(
[lambda x: from_list(Instance.from_dict, x), from_none],
obj.get("instances"),
)
physics = from_union([Physics.from_dict, from_none], obj.get("physics"))
return RobossemblerAssets(models, instances, physics)
def to_dict(self) -> dict:
result: dict = {}
if self.models is not None:
result["models"] = from_union(
[lambda x: from_list(lambda x: to_class(Model, x), x), from_none],
self.models,
)
if self.instances is not None:
result["instances"] = from_union(
[lambda x: from_list(lambda x: to_class(Instance, x), x), from_none],
self.instances,
)
if self.physics is not None:
result["physics"] = from_union(
[lambda x: to_class(Physics, x), from_none], self.physics
)
return result
def _getAllAtType(self, type: TypeEnum) -> List[Instance]:
return list(filter(lambda x: x.type == type, self.instances))
def getAllRobotsName(self) -> List[MappingInstanceAtModel]:
return list(map(lambda el: el, self._getAllAtType(type=TypeEnum.ROBOT)))
def getAllLightInstances(self) -> List[Instance]:
return list(
map(
lambda el: el.fromMappingInstanceAtModel(self.models),
self._getAllAtType(type=TypeEnum.LIGHT),
)
)
def getAllAssetsInstanceAtModel(self) -> List[MappingInstanceAtModel]:
return list(
map(
lambda el: el.fromMappingInstanceAtModel(self.models),
self._getAllAtType(type=TypeEnum.ASSET),
)
)

123
pddl/src/usecases/assembly_to_pddl_use_case.py
View file

@ -1,53 +1,86 @@
from typing import List
from helper.fs import FS
from unified_planning.shortcuts import *
from unified_planning import *
import os
from src.model.robossembler_assets import RobossemblerAssets
def doubleTagGenerate(instances: List[str], predicates: str, tag: str):
result = ""
for detail in instances:
result += "\n (" + predicates + " " + detail + " " + tag + ")" + "\n"
return result
def typeGenerator(types: list[str], typeName: str):
result = ""
for t in types:
result += t + " "
result += "- " + typeName
return result
def assemblyTagGenerate(sequences: list[str]):
result = ""
inc = 0
for el in sequences:
result += doubleTagGenerate(
instances=[el], predicates="part_of", tag="subasm" + str(inc)
)
inc += 1
inc = 0
for el in sequences[1 : sequences.__len__()]:
result += doubleTagGenerate(
instances=["subasm" + str(inc)],
predicates="assembly_order",
tag="subasm" + str(inc + 1),
)
inc += 1
return result
class AssemblyToPddlUseCase:
def call(assembly: List[str], rootLabel: str):
partType = UserType("part")
assemblyType = UserType('assembly')
objectsPartPddl = []
objectsAsmToPddl = []
i = 0
for el in assembly:
objectsPartPddl.append(Object(el, partType))
problem = Problem(rootLabel)
for el in objectsPartPddl:
problem.add_object(el)
i = 0
for el in objectsPartPddl:
problem.add_object(Object('subasm' + str(i), assemblyType))
objectsAsmToPddl.append(Object('subasm' + str(i), assemblyType))
i = i+1
connected = Fluent('part-of', BoolType(),
l_from=partType, l_to=assemblyType)
assemblyOrder = Fluent('assembly_order', BoolType(),
l_from=assemblyType, l_to=assemblyType)
i = 0
for el in objectsPartPddl:
problem.set_initial_value(connected(el, objectsAsmToPddl[i]), True)
i = i+1
goal = Fluent(rootLabel)
problem.add_goal(connected(objectsPartPddl[objectsPartPddl.__len__(
) - 1], objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]),)
i = 0
for el in objectsAsmToPddl:
if objectsAsmToPddl[i-1] != objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]:
problem.set_initial_value(assemblyOrder(objectsAsmToPddl[i-1], el), True)
i = i+1
problem.add_goal(assemblyOrder(objectsAsmToPddl[objectsAsmToPddl.__len__(
) - 1], objectsAsmToPddl[objectsAsmToPddl.__len__() - 1]),)
def call(assembly: List[str], robossemblerDb: RobossemblerAssets):
result = ""
robots = robossemblerDb.getAllRobotsName()
for robot in robots:
result += (
FS.readFile(
os.path.dirname(os.path.realpath(__file__))
+ "/../../mocks"
+ "/problem.txt"
)
.replace(
"${types}",
typeGenerator(assembly, "part")
+ "\n "
+ typeGenerator(
list(
map(
lambda x: "subasm" + str(x),
list(range(assembly.__len__())),
)
),
"assembly",
robot,
),
)
.replace(
"${assembled}",
assemblyTagGenerate(assembly),
)
.replace(
"${part_at}",
doubleTagGenerate(
instances=assembly, predicates="part_at", tag="workspace1"
),
)
).replace("${target}", "subasm" + str(assembly.__len__() - 1))
return {
"problem": unified_planning.io.PDDLWriter(problem).get_problem(),
'domain': FS.readFile(os.path.dirname(os.path.realpath(__file__)) + '/../../mocks' + '/domain.txt'),
"problem": result,
"domain": FS.readFile(
os.path.dirname(os.path.realpath(__file__))
+ "/../../mocks"
+ "/domain.txt"
),
}

17
pddl/unit.test.py
View file

@ -6,21 +6,28 @@ from helper.fs import FS
import os
mock = FS.readJSON(os.path.dirname(os.path.realpath(__file__)) + '/mocks/step-mock.json')
mock = FS.readJSON(
os.path.dirname(os.path.realpath(__file__)) + "/mocks/step-mock.json"
)
# assembly навык
assemblyToPddl = AssemblyToPddlUseCase.call(assembly=mock,rootLabel=mock[0])
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)
if __name__ == '__main__':
if __name__ == "__main__":
unittest.main()
# part_of определяет к какой подсборке относится деталь
# assembly определяет зоны
#

97
robossembler_scene_builder/main.py Normal file
View file

@ -0,0 +1,97 @@
from types import LambdaType, UnionType
from returns.pipeline import is_successful
from typing import List, TypeVar
from returns.result import Result, Success, Failure
import os
from model.robossembler_assets import (
MappingInstanceAtModel,
RobossemblerAssets,
Instance,
)
import re
import pathlib
from repository.file_system import FileSystemRepository
from model.robossembler_assets import Physics
from argparse import ArgumentParser
T = TypeVar("T")
class JsonReaderAndModelMapperUseCase:
def call(path: str, model: T) -> Result[T, str]:
try:
if not re.search("^(.+)\/([^\/]+)$", path):
return Failure("path not valid")
if model.from_dict == None:
return Failure("Model is not have mapping method from_dict")
return Success(model.from_dict(FileSystemRepository.readJSON(path=path)))
except:
return Failure("JsonReaderAndModelMapperUseCase unknown error")
class MappingInstanceAtModelToSdfUseCase:
def call(instances: List[MappingInstanceAtModel]) -> Result[List[str], str]:
try:
return Success(list(map(lambda el: el.toSDF(), instances)))
except:
return Failure("MappingInstanceAtModelToSdfUseCase unknown error")
class MappingSdfWorldToPhysicsModelUseCase:
def call(physicModel: Physics) -> Result[List[str], str]:
try:
return Success(Physics.toSDF(physicModel))
except:
return Failure("MappingInstanceAtModelToSdfUseCase unknown error")
class FormationOfTheSDFUseCase:
def call(worldTag: str, modelsTags: List[str], path: str) -> Result[bool, str]:
path = str(pathlib.Path(path).parent.resolve()) + "/"
if modelsTags == None:
return Failure("FormationOfTheSDFUseCase modelsTags is None")
if worldTag == None:
return Failure("FormationOfTheSDFUseCase worldTag is None")
FileSystemRepository.writeFile(
data=worldTag.replace("{models}", "\n".join(modelsTags)),
filePath=path,
fileName="world.sdf",
)
return Success(True)
def main():
parser = ArgumentParser()
parser.add_argument("--path", help="need path .json")
args = parser.parse_args()
if args.path == None:
parser.print_help()
return
path = args.path
jsonReaderAndModelMapperUseCase = JsonReaderAndModelMapperUseCase.call(
path=path, model=RobossemblerAssets
)
if not is_successful(jsonReaderAndModelMapperUseCase):
return
robossemblerAssets = jsonReaderAndModelMapperUseCase.value_or(None)
instanceSdfModel = MappingInstanceAtModelToSdfUseCase.call(
instances=robossemblerAssets.getAllAssetsInstanceAtModel()
)
sdfWorld = MappingSdfWorldToPhysicsModelUseCase.call(
physicModel=robossemblerAssets.physics
)
FormationOfTheSDFUseCase.call(
worldTag=sdfWorld.value_or(None),
modelsTags=instanceSdfModel.value_or(None),
path=path,
)
main()

12
robossembler_scene_builder/mocks/light_sdf.xml Normal file
View file

@ -0,0 +1,12 @@
<light type="{type_light}" name="{name_light}">
<pose>{x} {y} {z} {roll} {pitch} {yaw}</pose>
<diffuse>{r} {g} {b} {a}</diffuse>
<specular>.1 .1 .1 1</specular>
<attenuation>
<range>20</range>
<linear>0.2</linear>
<constant>0.8</constant>
<quadratic>0.01</quadratic>
</attenuation>
<cast_shadows>false</cast_shadows>
</light>

7
robossembler_scene_builder/mocks/model_include_sdf.xml Normal file
View file

@ -0,0 +1,7 @@
<model name="{name}">
<pose>{x} {y} {z} {roll} {pitch} {yaw}</pose>
<include>
<name>{name}</name>
<uri>model://{uri}</uri>
</include>
</model>

105
robossembler_scene_builder/mocks/world.xml Normal file
View file

@ -0,0 +1,105 @@
<?xml version="1.0"?>
<sdf version='1.9'>
<world name='mir'>
<physics name='1ms' type='{engine_type}'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</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' />
<plugin
name="ignition::gazebo::systems::Sensors" filename="ignition-gazebo-sensors-system">
<render_engine>ogre2</render_engine>
</plugin>
<gravity>{gravity_x} {gravity_y} {gravity_z}</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>false</shadows>
</scene>
<gui fullscreen="0">
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</ignition-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>1.0 1.0 1.0</ambient_light>
<background_color>0.4 0.6 1.0</background_color>
<camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
</plugin>
<plugin filename="WorldStats" name="World stats">
<ignition-gui>
<title>World stats</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">110</property>
<property type="double" key="width">290</property>
<property type="double" key="z">1</property>
<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="right" target="right" />
<line own="bottom" target="bottom" />
</anchors>
</ignition-gui>
<sim_time>true</sim_time>
<real_time>true</real_time>
<real_time_factor>true</real_time_factor>
<iterations>true</iterations>
</plugin>
</gui>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='ground'>
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</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>
</link>
</model>
{models} </world>
</sdf>

394
robossembler_scene_builder/model/robossembler_assets.py Normal file
View file

@ -0,0 +1,394 @@
from dataclasses import dataclass
import os
from returns.result import Result, Success, Failure
from typing import Optional, Any, List, TypeVar, Callable, Type, cast
from enum import Enum
from repository.file_system import FileSystemRepository
T = TypeVar("T")
EnumT = TypeVar("EnumT", bound=Enum)
def from_float(x: Any) -> float:
assert isinstance(x, (float, int)) and not isinstance(x, bool)
return float(x)
def from_none(x: Any) -> Any:
return x
def from_union(fs, x):
for f in fs:
try:
return f(x)
except:
pass
assert False
def to_float(x: Any) -> float:
assert isinstance(x, float)
return x
def from_str(x: Any) -> str:
assert isinstance(x, str)
return x
def from_int(x: Any) -> int:
assert isinstance(x, int) and not isinstance(x, bool)
return x
def from_list(f: Callable[[Any], T], x: Any) -> List[T]:
assert isinstance(x, list)
return [f(y) for y in x]
def to_class(c: Type[T], x: Any) -> dict:
assert isinstance(x, c)
return cast(Any, x).to_dict()
def to_enum(c: Type[EnumT], x: Any) -> EnumT:
assert isinstance(x, c)
return x.value
@dataclass
class Model:
name: Optional[str] = None
id: Optional[str] = None
path: Optional[str] = None
@staticmethod
def from_dict(obj: Any) -> "Model":
assert isinstance(obj, dict)
name = from_union([from_str, from_none], obj.get("name"))
id = from_union([from_str, from_none], obj.get("id"))
path = from_union([from_str, from_none], obj.get("path"))
return Model(name, id, path)
def to_dict(self) -> dict:
result: dict = {}
if self.name is not None:
result["name"] = from_union([from_str, from_none], self.name)
if self.id is not None:
result["id"] = from_union([from_str, from_none], self.id)
if self.path is not None:
result["path"] = from_union([from_str, from_none], self.path)
return result
@dataclass
class Pose:
x: Optional[float] = None
y: Optional[float] = None
z: Optional[float] = None
roll: Optional[float] = None
pitch: Optional[float] = None
yaw: Optional[float] = None
@staticmethod
def from_dict(obj: Any) -> "Pose":
assert isinstance(obj, dict)
x = from_union([from_float, from_none], obj.get("x"))
y = from_union([from_float, from_none], obj.get("y"))
z = from_union([from_float, from_none], obj.get("z"))
roll = from_union([from_float, from_none], obj.get("roll"))
pitch = from_union([from_float, from_none], obj.get("pitch"))
yaw = from_union([from_float, from_none], obj.get("yaw"))
return Pose(x, y, z, roll, pitch, yaw)
def to_dict(self) -> dict:
result: dict = {}
if self.x is not None:
result["x"] = from_union([to_float, from_none], self.x)
if self.y is not None:
result["y"] = from_union([to_float, from_none], self.y)
if self.z is not None:
result["z"] = from_union([to_float, from_none], self.z)
if self.roll is not None:
result["roll"] = from_union([to_float, from_none], self.roll)
if self.pitch is not None:
result["pitch"] = from_union([to_float, from_none], self.pitch)
if self.yaw is not None:
result["yaw"] = from_union([to_float, from_none], self.yaw)
return result
class TypeEnum(Enum):
ASSET = "asset"
LIGHT = "light"
@dataclass
class Instance:
model_name: Optional[str] = None
model_id: Optional[str] = None
id: Optional[str] = None
pose: Optional[Pose] = None
scale: Optional[int] = None
type: Optional[TypeEnum] = None
parent: Optional[str] = None
light_type: Optional[str] = None
intencity: Optional[int] = None
diffuse: Optional[List[float]] = None
spot_angle: Optional[int] = None
@staticmethod
def from_dict(obj: Any) -> "Instance":
assert isinstance(obj, dict)
model_name = from_union([from_str, from_none], obj.get("model_name"))
model_id = from_union([from_str, from_none], obj.get("model_id"))
id = from_union([from_str, from_none], obj.get("id"))
pose = from_union([Pose.from_dict, from_none], obj.get("pose"))
scale = from_union([from_int, from_none], obj.get("scale"))
type = from_union([TypeEnum, from_none], obj.get("type"))
parent = from_union([from_str, from_none], obj.get("parent"))
light_type = from_union([from_str, from_none], obj.get("light_type"))
intencity = from_union([from_int, from_none], obj.get("intencity"))
diffuse = from_union(
[lambda x: from_list(from_float, x), from_none], obj.get("diffuse")
)
spot_angle = from_union([from_int, from_none], obj.get("spot_angle"))
return Instance(
model_name,
model_id,
id,
pose,
scale,
type,
parent,
light_type,
intencity,
diffuse,
spot_angle,
)
def fromMappingInstanceAtModel(
self, models: List[Model]
) -> "MappingInstanceAtModel":
for el in models:
if el.id == self.model_id:
return MappingInstanceAtModel(instance=self, model=el)
return Failure("not found model at {self.model_id} ")
def to_dict(self) -> dict:
result: dict = {}
if self.model_name is not None:
result["model_name"] = from_union([from_str, from_none], self.model_name)
if self.model_id is not None:
result["model_id"] = from_union([from_str, from_none], self.model_id)
if self.id is not None:
result["id"] = from_union([from_str, from_none], self.id)
if self.pose is not None:
result["pose"] = from_union(
[lambda x: to_class(Pose, x), from_none], self.pose
)
if self.scale is not None:
result["scale"] = from_union([from_int, from_none], self.scale)
if self.type is not None:
result["type"] = from_union(
[lambda x: to_enum(TypeEnum, x), from_none], self.type
)
if self.parent is not None:
result["parent"] = from_union([from_str, from_none], self.parent)
if self.light_type is not None:
result["light_type"] = from_union([from_str, from_none], self.light_type)
if self.intencity is not None:
result["intencity"] = from_union([from_int, from_none], self.intencity)
if self.diffuse is not None:
result["diffuse"] = from_union(
[lambda x: from_list(to_float, x), from_none], self.diffuse
)
if self.spot_angle is not None:
result["spot_angle"] = from_union([from_int, from_none], self.spot_angle)
return result
class BasePose:
def __init__(self, x: float, y: float, z: float, **kwargs):
self.x = x
self.y = y
self.z = z
def toPose(self, sdfXmlMock: str):
return (
sdfXmlMock.replace("{x}", str(self.x))
.replace("{y}", str(self.y))
.replace("{z}", str(self.z))
)
class MappingInstanceAtModel(BasePose):
instance: Instance
model: Model
def __init__(self, instance: Instance, model: Model) -> None:
self.instance = instance
self.model = model
pass
def toSDF(self):
pose = self.instance.pose
match self.instance.type:
case TypeEnum.ASSET:
mock = FileSystemRepository.readFile(
os.path.dirname(os.path.realpath(__file__))
+ "/../mocks/model_include_sdf.xml"
)
# mockPose = self.toPose(mock)
return (
mock.replace("{name}", str(self.model.name))
.replace("{x}", str(pose.x))
.replace("{y}", str(pose.y))
.replace("{z}", str(pose.z))
.replace("{pitch}", str(pose.pitch))
.replace("{yaw}", str(pose.yaw))
.replace("{roll}", str(pose.roll))
.replace("{uri}", str(self.model.path))
)
case TypeEnum.LIGHT:
pathMock = (
os.path.dirname(os.path.realpath(__file__))
+ "/../mocks/light_sdf.xml"
)
return (
FileSystemRepository.readFile(pathMock)
.replace("{x}", str(pose.x))
.replace("{y}", str(pose.y))
.replace("{z}", str(pose.z))
.replace("{pitch}", str(pose.pitch))
.replace("{yaw}", str(pose.yaw))
.replace("{roll}", str(pose.roll))
.replace("{type_light}", str(self.instance.light_type))
.replace("{name_light}", str("132"))
.replace("{r}", self.instance.diffuse[0])
.replace("{g}", self.instance.diffuse[1])
.replace("{b}", self.instance.diffuse[2])
.replace("{a}", self.instance.diffuse[3])
)
@dataclass
class Gravity:
x: Optional[int] = None
y: Optional[int] = None
z: Optional[float] = None
@staticmethod
def from_dict(obj: Any) -> "Gravity":
assert isinstance(obj, dict)
x = from_union([from_int, from_none], obj.get("x"))
y = from_union([from_int, from_none], obj.get("y"))
z = from_union([from_float, from_none], obj.get("z"))
return Gravity(x, y, z)
def to_dict(self) -> dict:
result: dict = {}
if self.x is not None:
result["x"] = from_union([from_int, from_none], self.x)
if self.y is not None:
result["y"] = from_union([from_int, from_none], self.y)
if self.z is not None:
result["z"] = from_union([to_float, from_none], self.z)
return result
@dataclass
class Physics:
engine_name: Optional[str] = None
gravity: Optional[Gravity] = None
@staticmethod
def from_dict(obj: Any) -> "Physics":
assert isinstance(obj, dict)
engine_name = from_union([from_str, from_none], obj.get("engine_name"))
gravity = from_union([Gravity.from_dict, from_none], obj.get("gravity"))
return Physics(engine_name, gravity)
def to_dict(self) -> dict:
result: dict = {}
if self.engine_name is not None:
result["engine_name"] = from_union([from_str, from_none], self.engine_name)
if self.gravity is not None:
result["gravity"] = from_union(
[lambda x: to_class(Gravity, x), from_none], self.gravity
)
return result
def toSDF(self) -> str:
pathMock = os.path.dirname(os.path.realpath(__file__)) + "/../mocks/world.xml"
gravity = self.gravity
return (
FileSystemRepository.readFile(pathMock)
.replace("{gravity_x}", str(gravity.x))
.replace("{gravity_y}", str(gravity.y))
.replace("{gravity_z}", str(gravity.z))
.replace("{engine_type}", str(self.engine_name))
)
@dataclass
class RobossemblerAssets:
models: Optional[List[Model]] = None
instances: Optional[List[Instance]] = None
physics: Optional[Physics] = None
@staticmethod
def from_dict(obj: Any) -> "RobossemblerAssets":
assert isinstance(obj, dict)
models = from_union(
[lambda x: from_list(Model.from_dict, x), from_none], obj.get("models")
)
instances = from_union(
[lambda x: from_list(Instance.from_dict, x), from_none],
obj.get("instances"),
)
physics = from_union([Physics.from_dict, from_none], obj.get("physics"))
return RobossemblerAssets(models, instances, physics)
def to_dict(self) -> dict:
result: dict = {}
if self.models is not None:
result["models"] = from_union(
[lambda x: from_list(lambda x: to_class(Model, x), x), from_none],
self.models,
)
if self.instances is not None:
result["instances"] = from_union(
[lambda x: from_list(lambda x: to_class(Instance, x), x), from_none],
self.instances,
)
if self.physics is not None:
result["physics"] = from_union(
[lambda x: to_class(Physics, x), from_none], self.physics
)
return result
def _getAllAtType(self, type: TypeEnum) -> List[Instance]:
return list(filter(lambda x: x.type == type, self.instances))
def getAllLightInstances(self) -> List[Instance]:
return list(
map(
lambda el: el.fromMappingInstanceAtModel(self.models),
self._getAllAtType(type=TypeEnum.LIGHT),
)
)
def getAllAssetsInstanceAtModel(self) -> List[MappingInstanceAtModel]:
return list(
map(
lambda el: el.fromMappingInstanceAtModel(self.models),
self._getAllAtType(type=TypeEnum.ASSET),
)
)

25
robossembler_scene_builder/repository/file_system.py Normal file
View file

@ -0,0 +1,25 @@
import json
import os
class FileSystemRepository:
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", encoding="utf-8", errors="ignore")
f.write(data)
f.close()
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

8
stability_process_predicate/main.py
View file

@ -1,12 +1,14 @@
import argparse
from usecases.stability_check_usecase import StabilityCheckUseCase
# python3 main.py --aspPath /Users/idontsudo/framework/asp/out
# python3 main.py --aspPath /Users/idontsudo/Desktop/asp-example/
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--aspPath', help='asp folder generation path')
parser.add_argument("--aspPath", help="asp folder generation path")
args = parser.parse_args()
StabilityCheckUseCase().call(args.aspPath)
main()
main()

103
stability_process_predicate/usecases/stability_check_usecase.py
View file

@ -53,7 +53,7 @@ class Coords:
self.z = z
@staticmethod
def from_dict(obj: Any) -> 'Coords':
def from_dict(obj: Any) -> "Coords":
assert isinstance(obj, dict)
x = from_float(obj.get("x"))
y = from_float(obj.get("y"))
@ -79,7 +79,7 @@ class SimulatorStabilityResultModel:
self.position = position
@staticmethod
def from_dict(obj: Any) -> 'SimulatorStabilityResultModel':
def from_dict(obj: Any) -> "SimulatorStabilityResultModel":
assert isinstance(obj, dict)
id = from_str(obj.get("id"))
quaternion = Coords.from_dict(obj.get("quaternion"))
@ -103,90 +103,127 @@ def SimulatorStabilityModeltodict(x: List[SimulatorStabilityResultModel]) -> Any
class StabilityCheckUseCase:
def urdfLoader(self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str]):
def urdfLoader(
self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str]
):
urdfs = []
for assemblyCount in range(len(assembly)):
urdf = urdfGeneration.get(assembly[assemblyCount])
file_to_open = outPath + '/sdf-generation/' + \
str(assemblyCount) + '.urdf'
f = open(file_to_open, 'w', encoding='utf-8',
errors='ignore')
file_to_open = outPath + "/generation/" + str(assemblyCount) + ".urdf"
f = open(file_to_open, "w", encoding="utf-8", errors="ignore")
f.write(urdf)
f.close()
urdfs.append(os.path.abspath(f.name))
return urdfs
def executeSimulation(self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str], duration: int) -> list['SimulatorStabilityResultModel']:
def executeSimulation(
self,
assembly: list[str],
outPath: str,
urdfGeneration: dict[str:str],
duration: int,
) -> list["SimulatorStabilityResultModel"]:
p.connect(p.DIRECT)
p.setGravity(0, 0, -10)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
resultCoords = []
urdfs = self.urdfLoader(assembly=assembly,
urdfGeneration=urdfGeneration, outPath=outPath)
urdfs = self.urdfLoader(
assembly=assembly, urdfGeneration=urdfGeneration, outPath=outPath
)
bulletIds = []
for el in urdfs:
id = p.loadURDF(el)
bulletIds.append(id)
for i in range(duration):
if (i + 200 == duration):
if i + 200 == duration:
inc = 0
for bulletUUID in bulletIds:
pos, rot = p.getBasePositionAndOrientation(bulletUUID)
resultCoords.append(SimulatorStabilityResultModel(id=assembly[inc], quaternion=Coords(
x=rot[0], y=rot[1], z=rot[2]), position=Coords(x=pos[0], y=pos[1], z=pos[2])))
resultCoords.append(
SimulatorStabilityResultModel(
id=assembly[inc],
quaternion=Coords(x=rot[0], y=rot[1], z=rot[2]),
position=Coords(x=pos[0], y=pos[1], z=pos[2]),
)
)
p.removeBody(bulletUUID)
inc += 1
p.stepSimulation()
time.sleep(1./240.)
time.sleep(1.0 / 240.0)
return resultCoords
def call(self, aspPath: str):
try:
assemblyFolder = aspPath
assemblesStructures = json.loads(
(open(assemblyFolder + 'sequences.json')).read()).get('sequences')
(open(assemblyFolder + "sequences.json")).read()
).get("sequences")
tasks = len(assemblesStructures) * len(assemblesStructures[0])
taskCounter = 0
urdfGeneration = json.loads(
(open(assemblyFolder + 'sdf-generation/urdf-generation.json')).read())
(open(assemblyFolder + "generation/urdf-generation.json")).read()
)
for activeAssemblyNumber in range(len(assemblesStructures)):
pathSaveResultAssemblyFolder = aspPath + 'stability' + \
'/' + str(activeAssemblyNumber + 1) + '/'
pathSaveResultAssemblyFolder = (
aspPath + "stability" + "/" + str(activeAssemblyNumber + 1) + "/"
)
if not os.path.exists(pathSaveResultAssemblyFolder):
os.makedirs(pathSaveResultAssemblyFolder)
for subAssemblyNumber in range(len(assemblesStructures[activeAssemblyNumber])):
for subAssemblyNumber in range(
len(assemblesStructures[activeAssemblyNumber])
):
taskCounter += 1
subAssembly = assemblesStructures[activeAssemblyNumber][0:subAssemblyNumber+1]
subAssembly = assemblesStructures[activeAssemblyNumber][
0 : subAssemblyNumber + 1
]
print(subAssembly)
if subAssembly == [
"disk_top",
"disk_middel",
]:
asm = []
for el in subAssembly:
asm.append(el)
resultSimulationStates = self.executeSimulation(
assembly=asm, outPath=aspPath, urdfGeneration=urdfGeneration, duration=1000)
assembly=asm,
outPath=aspPath,
urdfGeneration=urdfGeneration,
duration=1000,
)
pathSaveResultSubAssemblyFolder = aspPath + 'stability' + '/' + \
str(activeAssemblyNumber + 1) + \
'/' + str(subAssemblyNumber) + '/'
pathSaveResultSubAssemblyFolder = (
aspPath
+ "stability"
+ "/"
+ str(activeAssemblyNumber + 1)
+ "/"
+ str(subAssemblyNumber)
+ "/"
)
if not os.path.exists(pathSaveResultSubAssemblyFolder):
os.makedirs(pathSaveResultSubAssemblyFolder)
results = {}
for state in resultSimulationStates:
results[state.id] = state.to_dict()
f = open(pathSaveResultSubAssemblyFolder + '/' +
'motion_result.json', 'w', encoding='utf-8', errors='ignore')
f.write(json.dumps(results,
ensure_ascii=False, indent=4))
f = open(
pathSaveResultSubAssemblyFolder
+ "/"
+ "motion_result.json",
"w",
encoding="utf-8",
errors="ignore",
)
f.write(json.dumps(results, ensure_ascii=False, indent=4))
f.close()
percentageOfCompletion = taskCounter / tasks * 100
print('process complete: ' +
str(percentageOfCompletion) + '%')
print("process complete: " + str(percentageOfCompletion) + "%")
except Exception as e:
print(e)